diff --git a/.gitignore b/.gitignore
index bd992a5421..8864c07043 100644
--- a/.gitignore
+++ b/.gitignore
@@ -3,6 +3,7 @@
*.out
*~
+*.swp
*.pyc
@@ -16,6 +17,24 @@
*.aux
+# Debian related files
+*.deb
+*.dsc
+*.changes
+*.substvars
+*.debhelper.log
+*-stamp
+/debian/control
+/debian/changelog
+/debian/files
+/debian/paparazzi-arm7
+/debian/paparazzi-avr
+/debian/paparazzi-dev
+/debian/paparazzi-bin
+/sw/lib/ocaml/ivy/debian/changelog
+/sw/lib/ocaml/ivy/debian/files
+/sw/lib/ocaml/ivy/debian/ivy-ocaml
+
/var
/dox
@@ -26,6 +45,7 @@
/conf/conf.xml.20*
/conf/control_panel.xml
/conf/%gconf.xml
+/conf/srtm_data/*
# /doc/pprz_algebra/
/doc/pprz_algebra/headfile.log
@@ -58,6 +78,10 @@
/sw/ground_segment/tmtc/server
/sw/ground_segment/tmtc/diadec
+# /sw/ground_segment/joystick
+/sw/ground_segment/joystick/input2ivy
+/sw/ground_segment/joystick/test_stick
+
# /sw/lib/ocaml/
/sw/lib/ocaml/gtk_papget_editor.ml
/sw/lib/ocaml/gtk_papget_text_editor.ml
@@ -88,3 +112,24 @@
# /sw/tools/
/sw/tools/fp_parser.ml
/sw/tools/wiki_gen/wiki_gen
+
+# /sw/ground_segment/misc
+/sw/ground_segment/misc/davis2ivy
+
+
+# /sw/airborne/arch/lpc21/test/bootloader
+/sw/airborne/arch/lpc21/test/bootloader/bl.dmp
+/sw/airborne/arch/lpc21/test/bootloader/bl.hex
+/sw/airborne/arch/lpc21/test/bootloader/bl.map
+/sw/airborne/arch/lpc21/test/bootloader/bl.elf
+/sw/airborne/arch/lpc21/test/bootloader/bl_ram.dmp
+/sw/airborne/arch/lpc21/test/bootloader/bl_ram.hex
+/sw/airborne/arch/lpc21/test/bootloader/bl_ram.map
+/sw/airborne/arch/lpc21/test/bootloader/bl_ram.elf
+/sw/airborne/arch/lpc21/test/bootloader/crt.lst
+
+# /sw/airborne/test
+/sw/airborne/test/test_geodetic
+/sw/airborne/test/test_algebra
+/sw/airborne/test/test_matrix
+/sw/airborne/test/test_att
diff --git a/Doxyfile b/Doxyfile
index ac741f1ee6..934897d58e 100644
--- a/Doxyfile
+++ b/Doxyfile
@@ -574,7 +574,7 @@ WARN_LOGFILE =
# directories like "/usr/src/myproject". Separate the files or directories
# with spaces.
-INPUT = sw/airborne
+INPUT = sw/airborne sw/airborne/math
# This tag can be used to specify the character encoding of the source files
# that doxygen parses. Internally doxygen uses the UTF-8 encoding, which is
diff --git a/Makefile b/Makefile
index accca2e3df..5e928b0fa7 100644
--- a/Makefile
+++ b/Makefile
@@ -43,6 +43,7 @@ AIRBORNE=sw/airborne
COCKPIT=sw/ground_segment/cockpit
TMTC=sw/ground_segment/tmtc
MULTIMON=sw/ground_segment/multimon
+MISC=sw/ground_segment/misc
LOGALIZER=sw/logalizer
SIMULATOR=sw/simulator
MAKE=make PAPARAZZI_SRC=$(PAPARAZZI_SRC) PAPARAZZI_HOME=$(PAPARAZZI_HOME)
@@ -69,7 +70,7 @@ OCAMLRUN=$(shell which ocamlrun)
all: commands static conf
-static : lib center tools cockpit multimon tmtc logalizer lpc21iap sim_static static_h usb_lib
+static : lib center tools cockpit multimon tmtc misc logalizer lpc21iap sim_static static_h usb_lib
conf: conf/conf.xml conf/control_panel.xml
@@ -98,13 +99,16 @@ cockpit: lib
tmtc: lib cockpit
cd $(TMTC); $(MAKE) all
+misc:
+ cd $(MISC); $(MAKE) all
+
multimon:
cd $(MULTIMON); $(MAKE)
static_h: $(MESSAGES_H) $(MESSAGES2_H) $(UBX_PROTOCOL_H) $(XSENS_PROTOCOL_H) $(DL_PROTOCOL_H) $(DL_PROTOCOL2_H)
usb_lib:
- @[ -d sw/airborne/arch/lpc21/lpcusb ] && ((test -x $(ARMGCC) && (cd sw/airborne/arch/lpc21/lpcusb; $(MAKE))) || echo "Not building usb_lib: ARMGCC=$(ARMGCC) not found") || echo "Not building usb_lib: sw/airborne/arch/lpc21/lpcusb directory missing"
+ @[ -d sw/airborne/arch/lpc21/lpcusb ] && ((test -x "$(ARMGCC)" && (cd sw/airborne/arch/lpc21/lpcusb; $(MAKE))) || echo "Not building usb_lib: ARMGCC=$(ARMGCC) not found") || echo "Not building usb_lib: sw/airborne/arch/lpc21/lpcusb directory missing"
$(MESSAGES_H) : $(MESSAGES_XML) $(CONF_XML) tools
$(Q)test -d $(STATICINCLUDE) || mkdir -p $(STATICINCLUDE)
@@ -220,7 +224,9 @@ cleanspaces:
find ./sw -name '*.mli' -exec sed -i {} -e 's/[ \t]*$$//' ';'
find ./conf -name '*.xml' -exec sed -i {} -e 's/[ \t]*$$//' ';'
+distclean : dist_clean
dist_clean : clean
+ rm -r conf/srtm_data
ab_clean:
@@ -241,3 +247,9 @@ paparazzi:
sw/simulator/launchsitl:
cat src/$(@F) | sed s#OCAMLRUN#$(OCAMLRUN)# | sed s#OCAML#$(OCAML)# > $@
chmod a+x $@
+
+#.SUFFIXES: .hgt.zip
+
+%.hgt.zip:
+ cd data/srtm; $(MAKE) $(@)
+
diff --git a/TODO b/TODO
deleted file mode 100644
index 5911b6afbd..0000000000
--- a/TODO
+++ /dev/null
@@ -1,134 +0,0 @@
-
-
-add date/time of generation in generated code
-
-##############################################################
-
-add an irc client to morphix - preconfigured for paparazzi channel
-
-
-
-make clean doesn't delete boa config
-
-make a "single node" mode where boa isn't needed and serve file:// URL
-force ivybus to 127 in single node
-default to that mode
-
-
-
-
-
-
-######################################## obsolete ? ##############
-
-dans flybywire
-chop servo ne depends pas du servo
-
-
-pour les missions
-possibilité de "transformer" (rotation Z, translation XYZ) une mission
-
-possibilitite de faire la meme chose pour une partie des waypoints (on en a une partie pour les evolutions et une partie pour le circuit d'atterissage. On deplace ceux du circuit d'atterissage pour qu'il colle a la piste. et on deplace ceux des evolution pour etre en face du jury :)
-
-
-On stocke la/les transformations et on peut avoir des missions communes muret/ricou
-
-des declarations de points locales aux blocs et des transformations par blocs
-
-
-faire medit avec visu3d
-
-proposer d'ajouter un waypoint en relatif par rapport a un autre - et en coordonnees polaires (dist, QDM)
-
-
-
-
-
-l'interface du captureur de video - c'est aussi visu3d . Il a une liste de textures (photos) et on peut les transformer. La description est sauvées dans un fichier xml et peut etre rechargée. les photos vont dans var/photos
-
-
-dans visu3d, il faudrait pouvoir pivoter en Z sur la position courante (en tenant compte du zoom )
-
-
-######################
-
-logger les simus comme les vols - y penser en refaisant receive - code commun
-
-Pour les simus a plusieurs avions, il doit y avoir partage d'un certain nombre d'informations entre les differentes instances des simu (par exemple le vent)
-
-Dans un circle, il faut afficher le QDR - calcul au sol??
-
-Pour les missions, il faudrait pouvoir dire . faire un cercle pendant 180° ou faire un cercle pendant n secondes. Il faudrait donc disposer du temps depuis le block et du de l'angle parcouru depuis le debut du cercle.
-C'est pour faire un palier en haut de la monté. Pour laisser le terme accumulateur se recaler avant la descente.
-
-###########################################
-
-Sujet : procedure automatique d'interuption de vol pour microdrone
-
--identifier des scenarios:
-
-cause de l'interuption : autonomie, meteo, defaillance systeme
-
--modeliser la zone d'evolution et les autres contraintres (systemes defaillants, meteo)
-
-(dans un meeting, on veut a tout prix eviter le public, les routes etc...)
-
-- initialisation
-- iteratif ?
-
-#######################################
-
-integrer les gazs pour estimer l'autonomie restante
-
-
-###########################################
-
-
-Sujet Drone Thales
-
-Les eleves (1A ou 2A) construisent un avion et apprennent a le faire voler d'ici juillet.
-
-commande PCB
-commande composants radiospare/melexys/coronis/ublox
-labo pour assembler (labo micro onde ?)
-
-commande garat (avion, moteur, servos, batteries, radiocommande etc....)
-achat de petit outillage (dremel, fer a souder etc...)
-assemblage au labo drone
-
-cours de pilotages sur le twinstar
-
-On leur donne les petits projets pendant l'année sur Paparazzi.
-
-
-
-############################################
-
-mettre les projets enac sur la page web enac
-
-############################################
-
-
-Pourquoi on ne laisse pas message.xml modifiable, pour les taux de telemesure par exemple.
-
-on split les Makefiles
-
-
-###########
-
-manque gerbmerge dans les dependance??? ha non ca n'existe pas... a packager !
-
-
->
->
-> - Pitch should more be in degree than in radian in flight plan files.
->It could be in the future version of Paparazzi.
->
->
-> - It's strange MIN_HEIGHT_CARROT and MAX_HEIGHT_CARROT are in the
->code. Shouldn't it be in a conf file so as to change it ?
->
-- In the same way, MIN_SPEED_FOR_TAKEOFF should be in the airframe
-conf file ?
-
-
diff --git a/conf/Makefile.lpc21 b/conf/Makefile.lpc21
index 334e243b74..7415bf1da3 100644
--- a/conf/Makefile.lpc21
+++ b/conf/Makefile.lpc21
@@ -96,7 +96,7 @@ CFLAGS += -ffunction-sections -fdata-sections
CFLAGS += -finline-limit=1200 --param inline-unit-growth=100
# flags only for C
-CFLAGS + = -Wstrict-prototypes -Wmissing-declarations
+CFLAGS += -Wstrict-prototypes -Wmissing-declarations
CFLAGS += -Wmissing-prototypes -Wnested-externs
CFLAGS += $(CSTANDARD)
CFLAGS += $($(TARGET).CFLAGS) $(LOCAL_CFLAGS)
diff --git a/conf/Makefile.stm32 b/conf/Makefile.stm32
index 4517382f6d..be9a360e33 100644
--- a/conf/Makefile.stm32
+++ b/conf/Makefile.stm32
@@ -38,7 +38,7 @@ OPT = s
#OPT = 0
# Programs location
-TOOLCHAIN_DIR=$(shell find -L /opt/paparazzi/stm32 ~/sat -maxdepth 1 -type d -name arm-none-eabi 2>/dev/null | head -n 1 | xargs dirname )
+TOOLCHAIN_DIR=$(shell find -L ~/sat /opt/paparazzi/stm32 -maxdepth 1 -type d -name arm-none-eabi 2>/dev/null | head -n 1 | xargs dirname )
GCC_BIN_DIR=$(TOOLCHAIN_DIR)/bin
GCC_LIB_DIR=$(TOOLCHAIN_DIR)/arm-none-eabi/lib
@@ -99,14 +99,21 @@ LDSCRIPT = $($(TARGET).LDSCRIPT)
endif
endif
-CFLAGS = -I. -I./$(ARCH) $(INCLUDES) -D__thumb2__ -Wall -c -msoft-float -O$(OPT)
+#UNAME = $(shell uname -s)
+MULTILIB = $(shell if $(CC) --print-multi-lib | grep thumb2 > /dev/null ; then echo "yes"; else echo "no"; fi)
+
+CFLAGS = -I. -I./$(ARCH) $(INCLUDES) -D__thumb2__ -Wall -msoft-float -O$(OPT)
CFLAGS += -Wl,gc-sections
CFLAGS += -mcpu=$(MCU) -mthumb -ansi
+ifeq ("$(MULTILIB)","yes")
+CFLAGS += -mfix-cortex-m3-ldrd
+endif
CFLAGS += -std=gnu99
#CFLAGS += -malignment-traps
-CFLAGS += -fno-common -g$(DEBUG)
+CFLAGS += -fno-common
+CFLAGS += -g$(DEBUG)
CFLAGS += -ffunction-sections -fdata-sections
-CFLAGS += -Wall -Wimplicit
+CFLAGS += -Wimplicit
CFLAGS += -Wcast-align
CFLAGS += -Wpointer-arith -Wswitch
CFLAGS += -Wredundant-decls -Wreturn-type -Wshadow -Wunused
@@ -120,11 +127,18 @@ CFLAGS += -Wswitch-default
CFLAGS += $($(TARGET).CFLAGS)
AFLAGS = -ahls -mapcs-32
+ifeq ("$(MULTILIB)","yes")
+AFLAGS += -mcpu=$(MCU) -mthumb
+endif
AFLAGS += -x assembler-with-cpp -Wa,-adhlns=$(OBJDIR)/$(<:.S=.lst),--g$(DEBUG)
+ifeq ("$(MULTILIB)","yes")
+LDFLAGS = -T$(LDSCRIPT) -nostartfiles -O$(OPT) --gc-sections -mthumb -march=armv7 -mfix-cortex-m3-ldrd -msoft-float
+else
LDFLAGS = -D__thumb2__ -T$(LDSCRIPT) -nostartfiles -L$(GCC_LIB_DIR) -O$(OPT) --gc-sections
+endif
LDFLAGS += -Wl,-Map=$(OBJDIR)/$(TARGET).map,--cref,--gc-sections
-LDLIBS += -lc -lm -lgcc -lcmsis -lstm32
+LDLIBS += -lc -lm -lgcc -lcmsis -lstm32 -lopencm3_stm32
CPFLAGS = -j .isr_vector -j .text -j .data
CPFLAGS_BIN = -Obinary
@@ -135,17 +149,28 @@ ODFLAGS = -S
# Default target.
-all: printcommands sizebefore build sizeafter
+all: printcommands printmultilib sizebefore build sizeafter
printcommands:
- $(Q)echo "Using CC = $(CC)"
- $(Q)echo "Using LD = $(LD)"
- $(Q)echo "Using CP = $(CP)"
- $(Q)echo "Using DMP = $(DMP)"
- $(Q)echo "Using NM = $(NM)"
- $(Q)echo "Using SIZE = $(SIZE)"
- $(Q)echo "Using RM = $(RM)"
- $(Q)echo "Using OOCD = $(OOCD)"
+ @echo "Using CC = $(CC)"
+ @echo "Using LD = $(LD)"
+ @echo "Using CP = $(CP)"
+ @echo "Using DMP = $(DMP)"
+ @echo "Using NM = $(NM)"
+ @echo "Using SIZE = $(SIZE)"
+ @echo "Using OOCD = $(OOCD)"
+ @echo "GCC version:"
+ @$(CC) --version
+ @echo "OOCD version:"
+ @$(OOCD) --version
+
+ifeq ("$(MULTILIB)","yes")
+printmultilib:
+ @echo "*** Using multilib ***"
+else
+printmultilib:
+ @echo "*** NOT using multilib ***"
+endif
build: elf bin hex
# lss sym
@@ -210,15 +235,17 @@ ifeq ($(FLASH_MODE),SERIAL)
upload: $(OBJDIR)/$(TARGET).bin
$(LOADER) -p /dev/ttyUSB0 -b 115200 -e -w -v $^
else ifeq ($(FLASH_MODE),JTAG)
-upload: $(OBJDIR)/$(TARGET).bin
- @echo -e " OOCD\t$<"
+upload: $(OBJDIR)/$(TARGET).elf
+ @echo " OOCD\t$<"
$(Q)$(OOCD) -f interface/$(OOCD_INTERFACE).cfg \
- -f board/$(OOCD_BOARD).cfg \
- -c init \
- -c "reset halt" \
- -c "flash write_image erase $(OBJDIR)/$(TARGET).bin 0x08000000" \
- -c reset \
- -c shutdown
+ -f board/$(OOCD_BOARD).cfg \
+ -c init \
+ -c "reset halt" \
+ -c "reset init" \
+ -c "stm32x mass_erase 0" \
+ -c "flash write_image $<" \
+ -c reset \
+ -c shutdown
else
upload:
@echo unknown flash_mode $(FLASH_MODE)
diff --git a/conf/airframes/ENAC/quadrotor/blender.xml b/conf/airframes/ENAC/quadrotor/blender.xml
index 72313de308..ee48120890 100644
--- a/conf/airframes/ENAC/quadrotor/blender.xml
+++ b/conf/airframes/ENAC/quadrotor/blender.xml
@@ -18,9 +18,6 @@
-
-
-
diff --git a/conf/airframes/ENAC/quadrotor/booz2_g1.xml b/conf/airframes/ENAC/quadrotor/booz2_g1.xml
index 0df16e15f0..b8acf7af5e 100644
--- a/conf/airframes/ENAC/quadrotor/booz2_g1.xml
+++ b/conf/airframes/ENAC/quadrotor/booz2_g1.xml
@@ -4,7 +4,9 @@
-
+
@@ -16,7 +18,7 @@
-
+
@@ -29,7 +31,9 @@
-
+
+
+
diff --git a/conf/airframes/Poine/beth.xml b/conf/airframes/Poine/beth.xml
index e8b8102ba5..aaa4ec37d5 100644
--- a/conf/airframes/Poine/beth.xml
+++ b/conf/airframes/Poine/beth.xml
@@ -126,12 +126,12 @@ main_stm32.srcs += lisa/lisa_overo_link.c lisa/arch/stm32/lisa_overo_link_arch.c
#booz IMU
#main_stm32.CFLAGS += -DIMU_TYPE_H=\"imu/imu_b2.h\"
-#main_stm32.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_MS2001
+#main_stm32.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_MS2100
#main_stm32.srcs += $(SRC_FIRMWARE)/imu.c
#main_stm32.CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ
#main_stm32.srcs += $(SRC_FIRMWARE)/imu/imu_b2.c $(SRC_FIRMWARE)/imu/arch/$(ARCH)/imu_b2_arch.c
#main_stm32.srcs += peripherals/max1168.c $(SRC_ARCH)/peripherals/max1168_arch.c
-#main_stm32.srcs += peripherals/ms2001.c $(SRC_ARCH)/peripherals/ms2001_arch.c
+#main_stm32.srcs += peripherals/ms2100.c $(SRC_ARCH)/peripherals/ms2100_arch.c
#main_stm32.srcs += math/pprz_trig_int.c
#crista IMU
diff --git a/conf/airframes/Poine/booz2_a7.xml b/conf/airframes/Poine/booz2_a7.xml
index 091401ebed..61428ed284 100644
--- a/conf/airframes/Poine/booz2_a7.xml
+++ b/conf/airframes/Poine/booz2_a7.xml
@@ -61,14 +61,6 @@
-
@@ -77,7 +69,7 @@
@@ -124,7 +116,7 @@
-
+
+ -->
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -155,21 +173,29 @@
+
+
+
+
+
+
-
+
-
+
+-->
-
-
+
+
+
+
+
+
@@ -201,29 +231,35 @@
-
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/Poine/booz2_a6.xml b/conf/airframes/Poine/h_hex.xml
similarity index 100%
rename from conf/airframes/Poine/booz2_a6.xml
rename to conf/airframes/Poine/h_hex.xml
diff --git a/conf/airframes/Poine/test_settings.xml b/conf/airframes/Poine/test_settings.xml
new file mode 100644
index 0000000000..644d4e99a7
--- /dev/null
+++ b/conf/airframes/Poine/test_settings.xml
@@ -0,0 +1,17 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/TU_Delft/MicrojetCDW.xml b/conf/airframes/TU_Delft/MicrojetCDW.xml
index 77b6fddac1..34d651058f 100644
--- a/conf/airframes/TU_Delft/MicrojetCDW.xml
+++ b/conf/airframes/TU_Delft/MicrojetCDW.xml
@@ -4,21 +4,21 @@
-
+
-
+
-
-
+
+
-
-
+
+
-
-
+
+
-
-
+
+
@@ -61,12 +61,12 @@
-
-
-
-
-
-
+
+
+
+
+
+
@@ -100,19 +100,18 @@
-
-
-
+
-
-
+
@@ -120,59 +119,58 @@
-
+
-
+
-
+
-
+
-
+
-
+
-
-
-
-
-
-
+
+
+
+
+
@@ -190,39 +188,39 @@
-
-
-
-
-
-
+
+
+
+
+
+
-
+
-
-
-
+
+
+
-
-
+
+
-
-
-
+
+
+
-
+
diff --git a/conf/airframes/airframe.dtd b/conf/airframes/airframe.dtd
index 960cf580dd..22808bfe3e 100644
--- a/conf/airframes/airframe.dtd
+++ b/conf/airframes/airframe.dtd
@@ -115,7 +115,9 @@ target CDATA #IMPLIED
location CDATA #IMPLIED>
+main_freq CDATA #IMPLIED
+target CDATA #IMPLIED>
+name CDATA #REQUIRED
+target CDATA #IMPLIED>
diff --git a/conf/airframes/booz2_flixr.xml b/conf/airframes/booz2_flixr.xml
index 38255e1429..323dc620f4 100644
--- a/conf/airframes/booz2_flixr.xml
+++ b/conf/airframes/booz2_flixr.xml
@@ -4,28 +4,21 @@
-
-
+
-
-
+
+
-
-
-
-
-
+
+
-
-
-
@@ -33,6 +26,8 @@
+
+
@@ -213,8 +208,7 @@
-
-
+
diff --git a/conf/airframes/esden/calib/aspirin_012.xml b/conf/airframes/esden/calib/aspirin_012.xml
new file mode 100644
index 0000000000..484ce882c4
--- /dev/null
+++ b/conf/airframes/esden/calib/aspirin_012.xml
@@ -0,0 +1,32 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/esden/lisa_m_pwm.xml b/conf/airframes/esden/lisa_m_pwm.xml
new file mode 100644
index 0000000000..3f78a121e1
--- /dev/null
+++ b/conf/airframes/esden/lisa_m_pwm.xml
@@ -0,0 +1,245 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/esden/lisa_pwm_aspirin.xml b/conf/airframes/esden/lisa_pwm_aspirin.xml
new file mode 100644
index 0000000000..8b4db240fd
--- /dev/null
+++ b/conf/airframes/esden/lisa_pwm_aspirin.xml
@@ -0,0 +1,265 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ -->
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/jsbsim.xml b/conf/airframes/jsbsim.xml
index 67fe2f0a1a..18dc8cc3ca 100644
--- a/conf/airframes/jsbsim.xml
+++ b/conf/airframes/jsbsim.xml
@@ -191,25 +191,43 @@
-
-#### Config for SITL simulation
-include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
+
+
+
+
-sim.CFLAGS += -DBOARD_CONFIG=\"boards/tiny_sim.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-#### Config for SITL simulation with JSBSim
-SRC_FIRMWARE = firmwares/fixedwing
-include $(PAPARAZZI_SRC)/conf/autopilot/sitl_jsbsim.makefile
+
+
+
+
+
+
-jsbsim.CFLAGS += -DBOARD_CONFIG=\"boards/tiny_sim.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-jsbsim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
-jsbsim.srcs += $(SIMDIR)/sim_ac_fw.c
-
-
diff --git a/conf/airframes/microjet_example.xml b/conf/airframes/microjet_example.xml
index efbed68ea1..405a1356c0 100644
--- a/conf/airframes/microjet_example.xml
+++ b/conf/airframes/microjet_example.xml
@@ -158,9 +158,18 @@
+
+
+
+
+
diff --git a/conf/airframes/mm/extra/probe_t.xml b/conf/airframes/mm/extra/probe_t.xml
index b9120d8065..217b5f9bbd 100644
--- a/conf/airframes/mm/extra/probe_t.xml
+++ b/conf/airframes/mm/extra/probe_t.xml
@@ -14,14 +14,10 @@
-
-
-
+
-
-
-
+
diff --git a/conf/airframes/mm/extra/turbine_trigger.xml b/conf/airframes/mm/extra/turbine_trigger.xml
index c02889cc5d..91c6cad9a1 100644
--- a/conf/airframes/mm/extra/turbine_trigger.xml
+++ b/conf/airframes/mm/extra/turbine_trigger.xml
@@ -7,14 +7,10 @@
-
-
-
+
-
-
-
+
diff --git a/conf/airframes/mm/fixed-wing/funjetmm.xml b/conf/airframes/mm/fixed-wing/funjetmm.xml
index aaef743332..7249478bc8 100644
--- a/conf/airframes/mm/fixed-wing/funjetmm.xml
+++ b/conf/airframes/mm/fixed-wing/funjetmm.xml
@@ -10,12 +10,14 @@
+
+
+
+
+
+
+
-
-
-
-
-
@@ -25,30 +27,22 @@
-
-
-
-
-
-
-
-
+
-
-
-
+
+
@@ -60,16 +54,14 @@
-
-
diff --git a/conf/airframes/mm/fixed-wing/fw_ins_arduimu.xml b/conf/airframes/mm/fixed-wing/fw_ins_arduimu.xml
index ced804cc9e..144fa3b68f 100644
--- a/conf/airframes/mm/fixed-wing/fw_ins_arduimu.xml
+++ b/conf/airframes/mm/fixed-wing/fw_ins_arduimu.xml
@@ -29,17 +29,13 @@
-
-
-
+
-
-
-
+
diff --git a/conf/airframes/test_hb.xml b/conf/airframes/test_hb.xml
index ec26252172..4d8b1c6795 100644
--- a/conf/airframes/test_hb.xml
+++ b/conf/airframes/test_hb.xml
@@ -16,7 +16,7 @@
-
+
diff --git a/conf/airframes/twinjet_example.xml b/conf/airframes/twinjet_example.xml
index 70fd68b0b7..b5974575d7 100644
--- a/conf/airframes/twinjet_example.xml
+++ b/conf/airframes/twinjet_example.xml
@@ -36,7 +36,7 @@
-
+
diff --git a/conf/autopilot/booz2_test_progs.makefile b/conf/autopilot/booz2_test_progs.makefile
index ec7cc603ac..ff3604c885 100644
--- a/conf/autopilot/booz2_test_progs.makefile
+++ b/conf/autopilot/booz2_test_progs.makefile
@@ -131,6 +131,29 @@ tunnel_bb.CFLAGS += -DUSE_LED
tunnel_bb.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.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 $(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 += sys_time.c $(SRC_ARCH)/sys_time_hw.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 $(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 += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
+usb_tunnel_1.srcs += mcu.c $(SRC_ARCH)/mcu_arch.c
+
#
# test GPS
@@ -189,6 +212,7 @@ test_usb.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))'
# -DTIME_LED=1
test_usb.CFLAGS += -DUSE_LED
test_usb.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.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 += $(SRC_ARCH)/mcu_periph/uart_arch.c
diff --git a/conf/autopilot/fixedwing.makefile b/conf/autopilot/fixedwing.makefile
index d4e98b98c4..139a7ca919 100644
--- a/conf/autopilot/fixedwing.makefile
+++ b/conf/autopilot/fixedwing.makefile
@@ -37,6 +37,9 @@ ifeq ($(TARGET),$(ACTUATOR_TARGET))
ifeq ($(BOARD),lisa_l)
include $(CFG_SHARED)/actuators_direct.makefile
endif
+ ifeq ($(BOARD),lisa_m)
+ include $(CFG_SHARED)/actuators_direct.makefile
+ endif
else
include $(CFG_SHARED)/$(ACTUATORS).makefile
diff --git a/conf/autopilot/lisa_l_test_progs.makefile b/conf/autopilot/lisa_l_test_progs.makefile
index 5f2261a54b..95aef22c04 100644
--- a/conf/autopilot/lisa_l_test_progs.makefile
+++ b/conf/autopilot/lisa_l_test_progs.makefile
@@ -196,11 +196,11 @@ test_rc_spektrum.ARCHDIR = $(ARCH)
test_rc_spektrum.CFLAGS += -I$(SRC_ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT
test_rc_spektrum.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_rc_spektrum.srcs += $(SRC_AIRBORNE)/mcu.c \
- $(SRC_ARCH)/mcu_arch.c \
- $(SRC_BOOZ_TEST)/booz2_test_radio_control.c \
- $(SRC_ARCH)/stm32_exceptions.c \
- $(SRC_ARCH)/stm32_vector_table.c
+test_rc_spektrum.srcs += $(SRC_AIRBORNE)/mcu.c \
+ $(SRC_ARCH)/mcu_arch.c \
+ test/subsystems/test_radio_control.c \
+ $(SRC_ARCH)/stm32_exceptions.c \
+ $(SRC_ARCH)/stm32_vector_table.c
test_rc_spektrum.CFLAGS += -DUSE_LED
test_rc_spektrum.srcs += $(SRC_ARCH)/led_hw.c
@@ -276,11 +276,11 @@ test_adc.ARCHDIR = $(ARCH)
test_adc.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
test_adc.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_adc.srcs = $(SRC_AIRBORNE)/mcu.c \
+test_adc.srcs = $(SRC_LISA)/test_adc.c \
+ $(SRC_AIRBORNE)/mcu.c \
$(SRC_ARCH)/mcu_arch.c \
- $(SRC_LISA)/test_adc.c \
- $(SRC_ARCH)/stm32_exceptions.c \
- $(SRC_ARCH)/stm32_vector_table.c
+ $(SRC_ARCH)/stm32_exceptions.c \
+ $(SRC_ARCH)/stm32_vector_table.c
test_adc.CFLAGS += -DUSE_LED
test_adc.srcs += $(SRC_ARCH)/led_hw.c
@@ -299,138 +299,175 @@ test_adc.srcs += downlink.c pprz_transport.c
test_adc.srcs += $(SRC_ARCH)/mcu_periph/adc_arch.c
test_adc.CFLAGS += -DUSE_AD1 -DUSE_AD1_1 -DUSE_AD1_2 -DUSE_AD1_3 -DUSE_AD1_4
+#test_adc.CFLAGS += -DUSE_AD1 -DUSE_AD1_3
test_adc.CFLAGS += -DUSE_ADC1_2_IRQ_HANDLER
+
+
#
-# test IMU b2
+# common test
#
# configuration
# SYS_TIME_LED
# MODEM_PORT
# MODEM_BAUD
#
+PERIODIC_FREQUENCY = 512
+
+COMMON_TEST_CFLAGS = -I$(SRC_FIRMWARE) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
+COMMON_TEST_CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
+COMMON_TEST_SRCS = $(SRC_AIRBORNE)/mcu.c \
+ $(SRC_ARCH)/mcu_arch.c \
+ $(SRC_ARCH)/stm32_exceptions.c \
+ $(SRC_ARCH)/stm32_vector_table.c
+COMMON_TEST_CFLAGS += -DUSE_LED
+COMMON_TEST_SRCS += $(SRC_ARCH)/led_hw.c
+COMMON_TEST_CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED)
+COMMON_TEST_CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./$(PERIODIC_FREQUENCY).))'
+COMMON_TEST_CFLAGS += -DPERIODIC_FREQUENCY=$(PERIODIC_FREQUENCY)
+COMMON_TEST_SRCS += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+COMMON_TEST_CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
+COMMON_TEST_SRCS += $(SRC_ARCH)/mcu_periph/uart_arch.c
+COMMON_TEST_CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
+COMMON_TEST_SRCS += downlink.c pprz_transport.c
+COMMON_TEST_SRCS += math/pprz_trig_int.c
+
+
+#
+# test IMU b2 v1.1
+#
+IMU_B2_CFLAGS = -DIMU_TYPE_H=\"subsystems/imu/imu_b2.h\"
+IMU_B2_CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_MS2100 -DIMU_B2_VERSION_1_1
+IMU_B2_SRCS = $(SRC_SUBSYSTEMS)/imu.c
+IMU_B2_CFLAGS += -DMAX_1168_DRDY_PORT=$(MAX_1168_DRDY_PORT)
+IMU_B2_CFLAGS += -DMAX_1168_DRDY_PORT_SOURCE=$(MAX_1168_DRDY_PORT_SOURCE)
+IMU_B2_CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ
+IMU_B2_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_b2.c $(SRC_ARCH)/subsystems/imu/imu_b2_arch.c
+IMU_B2_SRCS += peripherals/max1168.c $(SRC_ARCH)/peripherals/max1168_arch.c
+IMU_B2_SRCS += peripherals/ms2100.c $(SRC_ARCH)/peripherals/ms2100_arch.c
+
test_imu_b2.ARCHDIR = $(ARCH)
-test_imu_b2.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT
-test_imu_b2.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_imu_b2.srcs += $(SRC_AIRBORNE)/mcu.c \
- $(SRC_ARCH)/mcu_arch.c \
- $(SRC_BOOZ_TEST)/booz_test_imu.c \
- $(SRC_ARCH)/stm32_exceptions.c \
- $(SRC_ARCH)/stm32_vector_table.c
+test_imu_b2.srcs = test/subsystems/test_imu.c
+test_imu_b2.CFLAGS = $(COMMON_TEST_CFLAGS)
+test_imu_b2.srcs += $(COMMON_TEST_SRCS)
+test_imu_b2.CFLAGS += $(IMU_B2_CFLAGS)
+test_imu_b2.srcs += $(IMU_B2_SRCS)
-test_imu_b2.CFLAGS += -DUSE_LED
-test_imu_b2.srcs += $(SRC_ARCH)/led_hw.c
-test_imu_b2.CFLAGS += -DUSE_SYS_TIME
-test_imu_b2.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED)
-test_imu_b2.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))'
-test_imu_b2.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-test_imu_b2.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
-test_imu_b2.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_imu_b2.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-test_imu_b2.srcs += downlink.c pprz_transport.c
-
-test_imu_b2.srcs += math/pprz_trig_int.c
-
-test_imu_b2.CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_b2.h\"
-test_imu_b2.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_MS2001 -DIMU_B2_VERSION_1_1
-test_imu_b2.srcs += $(SRC_SUBSYSTEMS)/imu.c
-test_imu_b2.CFLAGS += -DMAX_1168_DRDY_PORT=$(MAX_1168_DRDY_PORT)
-test_imu_b2.CFLAGS += -DMAX_1168_DRDY_PORT_SOURCE=$(MAX_1168_DRDY_PORT_SOURCE)
-test_imu_b2.CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ
-test_imu_b2.srcs += $(SRC_SUBSYSTEMS)/imu/imu_b2.c $(SRC_ARCH)/subsystems/imu/imu_b2_arch.c
-test_imu_b2.srcs += peripherals/max1168.c $(SRC_ARCH)/peripherals/max1168_arch.c
-test_imu_b2.srcs += peripherals/ms2001.c $(SRC_ARCH)/peripherals/ms2001_arch.c
#
-# test IMU b2 1.2
-#
-# configuration
-# SYS_TIME_LED
-# MODEM_PORT
-# MODEM_BAUD
+# test IMU b2 v1.2
#
+IMU_B2_2_CFLAGS = -DIMU_TYPE_H=\"subsystems/imu/imu_b2.h\"
+IMU_B2_2_CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_HMC5843 -DIMU_B2_VERSION_1_2
+IMU_B2_2_SRCS = $(SRC_SUBSYSTEMS)/imu.c
+IMU_B2_2_CFLAGS += -DMAX_1168_DRDY_PORT=$(MAX_1168_DRDY_PORT)
+IMU_B2_2_CFLAGS += -DMAX_1168_DRDY_PORT_SOURCE=$(MAX_1168_DRDY_PORT_SOURCE)
+IMU_B2_2_CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ
+IMU_B2_2_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_b2.c $(SRC_ARCH)/subsystems/imu/imu_b2_arch.c
+IMU_B2_2_SRCS += peripherals/max1168.c $(SRC_ARCH)/peripherals/max1168_arch.c
+IMU_B2_2_CFLAGS += -DUSE_I2C2
+IMU_B2_2_SRCS += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c
+IMU_B2_2_SRCS += peripherals/hmc5843.c $(SRC_ARCH)/peripherals/hmc5843_arch.c
+IMU_B2_2_CFLAGS += -DUSE_EXTI9_5_IRQ # Mag Int on PB5
+
test_imu_b2_2.ARCHDIR = $(ARCH)
-test_imu_b2_2.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT
-test_imu_b2_2.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_imu_b2_2.srcs += $(SRC_AIRBORNE)/mcu.c \
- $(SRC_ARCH)/mcu_arch.c \
- $(SRC_BOOZ_TEST)/booz_test_imu.c \
- $(SRC_ARCH)/stm32_exceptions.c \
- $(SRC_ARCH)/stm32_vector_table.c
+test_imu_b2_2.srcs = test/subsystems/test_imu.c
+test_imu_b2_2.CFLAGS = $(COMMON_TEST_CFLAGS)
+test_imu_b2_2.srcs += $(COMMON_TEST_SRCS)
+test_imu_b2_2.CFLAGS += $(IMU_B2_2_CFLAGS)
+test_imu_b2_2.srcs += $(IMU_B2_2_SRCS)
-test_imu_b2_2.CFLAGS += -DUSE_LED
-test_imu_b2_2.srcs += $(SRC_ARCH)/led_hw.c
-test_imu_b2_2.CFLAGS += -DUSE_SYS_TIME
-test_imu_b2_2.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED)
-test_imu_b2_2.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))'
-test_imu_b2_2.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-test_imu_b2_2.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
-test_imu_b2_2.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_imu_b2_2.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-test_imu_b2_2.srcs += downlink.c pprz_transport.c
-
-test_imu_b2_2.srcs += math/pprz_trig_int.c
-
-test_imu_b2_2.CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_b2.h\"
-test_imu_b2_2.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_HMC5843 -DIMU_B2_VERSION_1_2
-test_imu_b2_2.srcs += $(SRC_SUBSYSTEMS)/imu.c
-test_imu_b2_2.CFLAGS += -DMAX_1168_DRDY_PORT=$(MAX_1168_DRDY_PORT)
-test_imu_b2_2.CFLAGS += -DMAX_1168_DRDY_PORT_SOURCE=$(MAX_1168_DRDY_PORT_SOURCE)
-test_imu_b2_2.CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ
-test_imu_b2_2.srcs += $(SRC_SUBSYSTEMS)/imu/imu_b2.c $(SRC_ARCH)/subsystems/imu/imu_b2_arch.c
-test_imu_b2_2.srcs += peripherals/max1168.c $(SRC_ARCH)/peripherals/max1168_arch.c
-test_imu_b2_2.CFLAGS += -DUSE_I2C2
-test_imu_b2_2.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c
-test_imu_b2_2.srcs += peripherals/hmc5843.c $(SRC_ARCH)/peripherals/hmc5843_arch.c
-test_imu_b2_2.CFLAGS += -DUSE_EXTI9_5_IRQ # Mag Int on PB5
#
# test IMU aspirin
#
+IMU_ASPIRIN_CFLAGS = -DIMU_TYPE_H=\"imu/imu_aspirin.h\" -DIMU_OVERRIDE_CHANNELS
+IMU_ASPIRIN_SRCS = $(SRC_SUBSYSTEMS)/imu.c \
+ $(SRC_SUBSYSTEMS)/imu/imu_aspirin.c \
+ $(SRC_ARCH)/subsystems/imu/imu_aspirin_arch.c
+IMU_ASPIRIN_SRCS += peripherals/hmc5843.c $(SRC_ARCH)/peripherals/hmc5843_arch.c
+IMU_ASPIRIN_CFLAGS += -DUSE_I2C2
+IMU_ASPIRIN_SRCS += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c
+IMU_ASPIRIN_CFLAGS += -DUSE_EXTI15_10_IRQ # Gyro Int on PC14
+IMU_ASPIRIN_CFLAGS += -DUSE_EXTI9_5_IRQ # Mag Int on PB5
+IMU_ASPIRIN_CFLAGS += -DUSE_EXTI2_IRQ # Accel Int on PD2
+IMU_ASPIRIN_CFLAGS += -DUSE_DMA1_C4_IRQ # SPI2 Rx DMA
+
test_imu_aspirin.ARCHDIR = $(ARCH)
-test_imu_aspirin.CFLAGS = -I$(SRC_FIRMWARE) -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT
-test_imu_aspirin.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_imu_aspirin.srcs += $(SRC_AIRBORNE)/mcu.c \
- $(SRC_ARCH)/mcu_arch.c \
- $(SRC_BOOZ_TEST)/booz_test_imu.c \
- $(SRC_ARCH)/stm32_exceptions.c \
- $(SRC_ARCH)/stm32_vector_table.c
+test_imu_aspirin.srcs = test/subsystems/test_imu.c
+test_imu_aspirin.CFLAGS = $(COMMON_TEST_CFLAGS)
+test_imu_aspirin.srcs += $(COMMON_TEST_SRCS)
+test_imu_aspirin.CFLAGS += $(IMU_ASPIRIN_CFLAGS)
+test_imu_aspirin.srcs += $(IMU_ASPIRIN_SRCS)
-test_imu_aspirin.CFLAGS += -DUSE_LED
-test_imu_aspirin.srcs += $(SRC_ARCH)/led_hw.c
-test_imu_aspirin.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_imu_aspirin.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))'
-test_imu_aspirin.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+#
+# test AHRS
+#
+test_ahrs.ARCHDIR = $(ARCH)
+test_ahrs.srcs = test/subsystems/test_ahrs.c
+test_ahrs.CFLAGS = $(COMMON_TEST_CFLAGS)
+test_ahrs.srcs += $(COMMON_TEST_SRCS)
+test_ahrs.CFLAGS += $(IMU_ASPIRIN_CFLAGS)
+test_ahrs.srcs += $(IMU_ASPIRIN_SRCS)
-test_imu_aspirin.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
-test_imu_aspirin.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
+#AHRS = ice
+AHRS = icq
+#AHRS = flq
+#AHRS = fcr
+#AHRS = fcr2
+#AHRS = fcq
-test_imu_aspirin.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-test_imu_aspirin.srcs += downlink.c pprz_transport.c
+test_ahrs.srcs += subsystems/ahrs.c \
+ subsystems/ahrs/ahrs_aligner.c
-test_imu_aspirin.srcs += math/pprz_trig_int.c
+ifeq ($(AHRS), ice)
+test_ahrs.CFLAGS += -DFACE_REINJ_1=1024
+test_ahrs.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_euler.h\"
+test_ahrs.srcs += subsystems/ahrs/ahrs_int_cmpl_euler.c \
+ lisa/plug_sys.c
+endif
-test_imu_aspirin.CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin.h\" -DIMU_OVERRIDE_CHANNELS
-test_imu_aspirin.srcs += $(SRC_SUBSYSTEMS)/imu.c \
- $(SRC_SUBSYSTEMS)/imu/imu_aspirin.c \
- $(SRC_ARCH)/subsystems/imu/imu_aspirin_arch.c
-test_imu_aspirin.srcs += peripherals/hmc5843.c $(SRC_ARCH)/peripherals/hmc5843_arch.c
+ifeq ($(AHRS), icq)
+#test_ahrs.CFLAGS += -DAHRS_TYPE=\"ICQ\"
+test_ahrs.CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=512
+test_ahrs.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl.h\"
+test_ahrs.srcs +=subsystems/ahrs/ahrs_int_cmpl.c
+endif
-test_imu_aspirin.CFLAGS += -DUSE_I2C2
-test_imu_aspirin.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c
-test_imu_aspirin.CFLAGS += -DUSE_EXTI15_10_IRQ # Gyro Int on PC14
-test_imu_aspirin.CFLAGS += -DUSE_EXTI9_5_IRQ # Mag Int on PB5
-test_imu_aspirin.CFLAGS += -DUSE_EXTI2_IRQ # Accel Int on PD2
-test_imu_aspirin.CFLAGS += -DUSE_DMA1_C4_IRQ # SPI2 Rx DMA
+ifeq ($(AHRS), flq)
+test_ahrs.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_lkf_quat.h\"
+test_ahrs.CFLAGS += -DAHRS_H_X=0.51562740288882 -DAHRS_H_Y=-0.05707735220832 -DAHRS_H_Z=0.85490967783446
+test_ahrs.srcs += subsystems/ahrs/ahrs_float_lkf_quat.c
+endif
+ifeq ($(AHRS), fcr)
+test_ahrs.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm.h\"
+test_ahrs.CFLAGS += -DINS_ROLL_NEUTRAL_DEFAULT=0
+test_ahrs.CFLAGS += -DINS_PITCH_NEUTRAL_DEFAULT=0
+test_ahrs.CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=512
+test_ahrs.CFLAGS += -DDCM_UPDATE_AFTER_PROPAGATE
+test_ahrs.srcs += subsystems/ahrs/ahrs_float_dcm.c
+endif
+
+ifeq ($(AHRS), fcr2)
+test_ahrs.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_rmat.h\"
+test_ahrs.CFLAGS += -DAHRS_H_X=0.51562740288882 -DAHRS_H_Y=-0.05707735220832 -DAHRS_H_Z=0.85490967783446
+test_ahrs.CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=512
+test_ahrs.srcs += subsystems/ahrs/ahrs_float_cmpl_rmat.c
+endif
+
+ifeq ($(AHRS), fcq)
+test_ahrs.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_rmat.h\"
+test_ahrs.CFLAGS += -DAHRS_H_X=0.51562740288882 -DAHRS_H_Y=-0.05707735220832 -DAHRS_H_Z=0.85490967783446
+test_ahrs.CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=512
+test_ahrs.srcs += subsystems/ahrs/ahrs_float_cmpl_quat.c
+endif
@@ -663,65 +700,74 @@ test_bmp085.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c
+
#
-# Test manual : a simple test with rc and servos - I want to fly lisa/M
+# tunnel sw
#
-test_manual.ARCHDIR = $(ARCH)
-test_manual.CFLAGS = -I$(SRC_FIRMWARE) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
-test_manual.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_manual.srcs = $(SRC_AIRBORNE)/mcu.c \
- $(SRC_ARCH)/mcu_arch.c \
- test/test_manual.c \
- $(SRC_ARCH)/stm32_exceptions.c \
- $(SRC_ARCH)/stm32_vector_table.c
-test_manual.CFLAGS += -DUSE_LED
-test_manual.srcs += $(SRC_ARCH)/led_hw.c
-test_manual.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED)
-test_manual.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-test_manual.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-
-test_manual.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
-test_manual.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_manual.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
-test_manual.srcs += downlink.c pprz_transport.c
-
-test_manual.srcs += $(SRC_BOOZ)/booz2_commands.c
-
-test_manual.CFLAGS += -I$(SRC_FIRMWARE)/actuators/arch/$(ARCH)
-#test_manual.srcs += $(SRC_FIRMWARE)/actuators/actuators_pwm.c
-test_manual.srcs += $(SRC_FIRMWARE)/actuators/arch/$(ARCH)/actuators_pwm_arch.c
-test_manual.srcs += $(SRC_FIRMWARE)/actuators/actuators_heli.c
+tunnel_sw.ARCHDIR = $(ARCH)
+tunnel_sw.CFLAGS += -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
+tunnel_sw.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
+tunnel_sw.srcs += $(SRC_AIRBORNE)/mcu.c \
+ $(SRC_ARCH)/mcu_arch.c \
+ $(SRC_LISA)/tunnel_hw.c \
+ $(SRC_ARCH)/stm32_exceptions.c \
+ $(SRC_ARCH)/stm32_vector_table.c
+tunnel_sw.CFLAGS += -DUSE_LED
+tunnel_sw.srcs += $(SRC_ARCH)/led_hw.c
+tunnel_sw.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED)
+tunnel_sw.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
+tunnel_sw.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-test_manual.CFLAGS += -I$(SRC_BOOZ) -I$(SRC_BOOZ)/arch/$(ARCH)
-test_manual.CFLAGS += -DRADIO_CONTROL
-ifdef RADIO_CONTROL_LED
-test_manual.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED)
-endif
-test_manual.CFLAGS += -DRADIO_CONTROL_BIND_IMPL_FUNC=radio_control_spektrum_try_bind
-test_manual.CFLAGS += -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/spektrum.h\"
-test_manual.CFLAGS += -DRADIO_CONTROL_SPEKTRUM_PRIMARY_PORT=$(RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT)
-test_manual.CFLAGS += -DOVERRIDE_$(RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT)_IRQ_HANDLER -DUSE_TIM6_IRQ
-test_manual.srcs += $(SRC_SUBSYSTEMS)/radio_control.c \
- subsystems/radio_control/spektrum.c \
- $(SRC_ARCH)/subsystems/radio_control/spektrum_arch.c
+#
+# tunnel hw
+#
+tunnel_hw.ARCHDIR = $(ARCH)
+tunnel_hw.CFLAGS += -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
+tunnel_hw.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
+tunnel_hw.srcs += lisa/test/lisa_tunnel.c \
+ $(SRC_AIRBORNE)/mcu.c \
+ $(SRC_ARCH)/mcu_arch.c \
+ $(SRC_ARCH)/stm32_exceptions.c \
+ $(SRC_ARCH)/stm32_vector_table.c
+tunnel_hw.CFLAGS += -DUSE_LED
+tunnel_hw.srcs += $(SRC_ARCH)/led_hw.c
+tunnel_hw.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED)
+tunnel_hw.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
+tunnel_hw.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+tunnel_hw.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
+tunnel_hw.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
+tunnel_hw.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
#
-# tunnel
+# test_settings :
#
-tunnel.ARCHDIR = $(ARCH)
-tunnel.CFLAGS += -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
-tunnel.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-tunnel.srcs += $(SRC_AIRBORNE)/mcu.c \
- $(SRC_ARCH)/mcu_arch.c \
- $(SRC_LISA)/tunnel_hw.c \
- $(SRC_ARCH)/stm32_exceptions.c \
- $(SRC_ARCH)/stm32_vector_table.c
-tunnel.CFLAGS += -DUSE_LED
-tunnel.srcs += $(SRC_ARCH)/led_hw.c
-tunnel.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED)
-tunnel.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-tunnel.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+# configuration
+# MODEM_PORT :
+# MODEM_BAUD :
+#
+test_settings.ARCHDIR = $(ARCH)
+test_settings.CFLAGS += -I$(SRC_LISA) -I$(SRC_ARCH) -DPERIPHERALS_AUTO_INIT
+test_settings.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
+test_settings.srcs = test/subsystems/test_settings.c \
+ $(SRC_AIRBORNE)/mcu.c \
+ $(SRC_ARCH)/mcu_arch.c \
+ $(SRC_ARCH)/stm32_exceptions.c \
+ $(SRC_ARCH)/stm32_vector_table.c
+test_settings.CFLAGS += -DUSE_LED
+test_settings.srcs += $(SRC_ARCH)/led_hw.c
+test_settings.CFLAGS += -DUSE_SYS_TIME
+test_settings.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
+test_settings.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED)
+test_settings.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+test_settings.CFLAGS += -DUSE_$(MODEM_PORT)
+test_settings.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
+test_settings.srcs += downlink.c pprz_transport.c
+test_settings.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
+test_settings.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
+test_settings.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=$(MODEM_PORT)
+test_settings.srcs += subsystems/settings.c
+test_settings.srcs += $(SRC_ARCH)/subsystems/settings_arch.c
+test_settings.CFLAGS += -DUSE_PERSISTENT_SETTINGS
diff --git a/conf/autopilot/lisa_m_test_progs.makefile b/conf/autopilot/lisa_m_test_progs.makefile
new file mode 100644
index 0000000000..9fa3bd820f
--- /dev/null
+++ b/conf/autopilot/lisa_m_test_progs.makefile
@@ -0,0 +1,719 @@
+# Hey Emacs, this is a -*- makefile -*-
+#
+# $Id$
+# Copyright (C) 2010 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.
+#
+#
+
+
+
+
+################################################################################
+#
+#
+# Test program for the Lisa/M board
+#
+#
+#
+#
+# every "firmware" makefile should have a description of available targets
+# possible options for each of them, susbsystems and associated params for each of them
+#
+#
+#
+#
+################################################################################
+
+ARCH=stm32
+SRC_ARCH=arch/$(ARCH)
+SRC_LISA=lisa
+SRC_LISA_ARCH=$(SRC_LISA)/arch/$(ARCH)
+#SRC_ROTORCRAFT=rotorcraft
+SRC_BOARD=boards/$(BOARD)
+
+SRC_FIRMWARE=firmwares/rotorcraft
+SRC_SUBSYSTEMS=subsystems
+SRC_AIRBORNE=.
+
+#
+# default configuration expected from the board files
+#
+SYS_TIME_LED = 1
+# MODEM_PORT = UART2
+# MODEM_BAUD = B57600
+
+#
+# test leds
+#
+test_led.ARCHDIR = $(ARCH)
+test_led.CFLAGS += -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
+test_led.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
+test_led.srcs += $(SRC_AIRBORNE)/mcu.c \
+ $(SRC_ARCH)/mcu_arch.c \
+ $(SRC_LISA)/test_led.c \
+ $(SRC_ARCH)/stm32_exceptions.c \
+ $(SRC_ARCH)/stm32_vector_table.c
+test_led.CFLAGS += -DUSE_LED
+test_led.srcs += $(SRC_ARCH)/led_hw.c
+
+#
+# test uart
+#
+test_uart_lisam.ARCHDIR = $(ARCH)
+test_uart_lisam.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
+test_uart_lisam.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
+test_uart_lisam.srcs = $(SRC_AIRBORNE)/mcu.c \
+ $(SRC_ARCH)/mcu_arch.c \
+ $(SRC_LISA)/test_uart_lisam.c \
+ $(SRC_ARCH)/stm32_exceptions.c \
+ $(SRC_ARCH)/stm32_vector_table.c
+test_uart_lisam.CFLAGS += -DUSE_LED -DUSE_UART
+test_uart_lisam.srcs += $(SRC_ARCH)/led_hw.c
+test_uart_lisam.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED)
+test_uart_lisam.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
+test_uart_lisam.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+test_uart_lisam.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
+test_uart_lisam.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
+test_uart_lisam.CFLAGS += -DUSE_UART3 -DUART3_BAUD=B57600
+test_uart_lisam.CFLAGS += -DUSE_UART5 -DUART5_BAUD=B57600
+test_uart_lisam.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
+
+
+##
+## test servos
+##
+#
+#SRC_BOOZ_ARCH=$(SRC_BOOZ)/arch/$(ARCH)
+#
+test_servos.ARCHDIR = $(ARCH)
+test_servos.CFLAGS = -I$(SRC_FIRMWARE)/actuators/arch/$(ARCH) -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
+test_servos.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
+test_servos.LDFLAGS += -lm
+test_servos.srcs += $(SRC_AIRBORNE)/mcu.c \
+ $(SRC_ARCH)/mcu_arch.c \
+ $(SRC_LISA)/test_servos.c \
+ $(SRC_ARCH)/stm32_exceptions.c \
+ $(SRC_ARCH)/stm32_vector_table.c
+test_servos.CFLAGS += -DUSE_LED
+test_servos.srcs += $(SRC_ARCH)/led_hw.c
+test_servos.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED) -DUSE_SERVOS_7AND8
+test_servos.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))'
+test_servos.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+
+test_servos.srcs += $(SRC_FIRMWARE)/actuators/actuators_pwm.c $(SRC_FIRMWARE)/actuators/arch/$(ARCH)/actuators_pwm_arch.c
+#
+#
+##
+## test_telemetry : Sends ALIVE telemetry messages
+##
+## configuration
+## MODEM_PORT :
+## MODEM_BAUD :
+##
+test_telemetry.ARCHDIR = $(ARCH)
+test_telemetry.CFLAGS += -I$(SRC_LISA) -I$(SRC_ARCH) -DPERIPHERALS_AUTO_INIT
+test_telemetry.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
+test_telemetry.srcs = $(SRC_AIRBORNE)/mcu.c \
+ $(SRC_ARCH)/mcu_arch.c \
+ test/test_telemetry.c \
+ $(SRC_ARCH)/stm32_exceptions.c \
+ $(SRC_ARCH)/stm32_vector_table.c
+test_telemetry.CFLAGS += -DUSE_LED
+test_telemetry.srcs += $(SRC_ARCH)/led_hw.c
+test_telemetry.CFLAGS += -DUSE_SYS_TIME
+test_telemetry.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
+test_telemetry.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED)
+test_telemetry.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+test_telemetry.CFLAGS += -DUSE_$(MODEM_PORT)
+test_telemetry.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
+test_telemetry.srcs += downlink.c pprz_transport.c
+test_telemetry.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
+test_telemetry.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
+#
+#
+##
+## test_baro : reads barometers and sends values over telemetry
+##
+## configuration
+## SYS_TIME_LED
+## MODEM_PORT
+## MODEM_BAUD
+##
+test_baro.ARCHDIR = $(ARCH)
+test_baro.CFLAGS = -I$(SRC_LISA) -I$(SRC_ARCH) -I$(SRC_BOARD) -DPERIPHERALS_AUTO_INIT
+test_baro.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
+test_baro.srcs = $(SRC_AIRBORNE)/mcu.c \
+ $(SRC_ARCH)/mcu_arch.c \
+ $(SRC_BOARD)/test_baro.c \
+ $(SRC_ARCH)/stm32_exceptions.c \
+ $(SRC_ARCH)/stm32_vector_table.c
+test_baro.CFLAGS += -DUSE_LED
+test_baro.srcs += $(SRC_ARCH)/led_hw.c
+test_baro.CFLAGS += -DUSE_SYS_TIME
+test_baro.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
+test_baro.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED)
+test_baro.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+test_baro.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
+test_baro.srcs += downlink.c pprz_transport.c
+test_baro.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
+test_baro.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
+test_baro.srcs += $(SRC_BOARD)/baro_board.c
+test_baro.CFLAGS += -DUSE_I2C2
+test_baro.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c
+#
+#
+##
+## test_rc_spektrum : sends RADIO_CONTROL messages on telemetry
+##
+## configuration
+## SYS_TIME_LED
+## MODEM_PORT
+## MODEM_BAUD
+## RADIO_CONTROL_LED
+## RADIO_CONROL_SPEKTRUM_PRIMARY_PORT
+##
+test_rc_spektrum.ARCHDIR = $(ARCH)
+
+test_rc_spektrum.CFLAGS += -I$(SRC_ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT
+test_rc_spektrum.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
+test_rc_spektrum.srcs += $(SRC_AIRBORNE)/mcu.c \
+ $(SRC_ARCH)/mcu_arch.c \
+ booz/test/booz2_test_radio_control.c \
+ $(SRC_ARCH)/stm32_exceptions.c \
+ $(SRC_ARCH)/stm32_vector_table.c
+
+test_rc_spektrum.CFLAGS += -DUSE_LED
+test_rc_spektrum.srcs += $(SRC_ARCH)/led_hw.c
+test_rc_spektrum.CFLAGS += -DUSE_SYS_TIME
+test_rc_spektrum.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))'
+test_rc_spektrum.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED)
+test_rc_spektrum.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+test_rc_spektrum.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
+test_rc_spektrum.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
+test_rc_spektrum.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
+test_rc_spektrum.srcs += downlink.c pprz_transport.c
+test_rc_spektrum.CFLAGS += -DRADIO_CONTROL
+ifdef RADIO_CONTROL_LED
+test_rc_spektrum.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED)
+endif
+test_rc_spektrum.CFLAGS += -DRADIO_CONTROL_BIND_IMPL_FUNC=radio_control_spektrum_try_bind
+test_rc_spektrum.CFLAGS += -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/spektrum.h\"
+test_rc_spektrum.CFLAGS += -DRADIO_CONTROL_SPEKTRUM_PRIMARY_PORT=$(RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT)
+test_rc_spektrum.CFLAGS += -DRADIO_CONTROL_SPEKTRUM_SECONDARY_PORT=$(RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT)
+test_rc_spektrum.CFLAGS += -DOVERRIDE_$(RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT)_IRQ_HANDLER -DUSE_TIM6_IRQ
+test_rc_spektrum.CFLAGS += -DOVERRIDE_$(RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT)_IRQ_HANDLER
+test_rc_spektrum.srcs += $(SRC_SUBSYSTEMS)/radio_control.c \
+ subsystems/radio_control/spektrum.c \
+ $(SRC_ARCH)/subsystems/radio_control/spektrum_arch.c
+
+
+##
+## test_rc_ppm
+##
+## configuration
+## SYS_TIME_LED
+## MODEM_PORT
+## MODEM_BAUD
+## RADIO_CONTROL_LED
+##
+#test_rc_ppm.ARCHDIR = $(ARCH)
+#
+#test_rc_ppm.CFLAGS += -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -I$(SRC_BOARD)
+#test_rc_ppm.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
+#test_rc_ppm.CFLAGS += -DPERIPHERALS_AUTO_INIT
+#test_rc_ppm.srcs += $(SRC_AIRBORNE)/mcu.c \
+# $(SRC_ARCH)/mcu_arch.c \
+# $(SRC_BOOZ)/test/booz2_test_radio_control.c \
+# $(SRC_ARCH)/stm32_exceptions.c \
+# $(SRC_ARCH)/stm32_vector_table.c
+#
+#test_rc_ppm.CFLAGS += -DUSE_LED
+#test_rc_ppm.srcs += $(SRC_ARCH)/led_hw.c
+#test_rc_ppm.CFLAGS += -DUSE_SYS_TIME
+#test_rc_ppm.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))'
+#test_rc_ppm.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED)
+#test_rc_ppm.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+#test_rc_ppm.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
+#test_rc_ppm.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
+#test_rc_ppm.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
+#test_rc_ppm.srcs += downlink.c pprz_transport.c
+#test_rc_ppm.CFLAGS += -DRADIO_CONTROL
+#test_rc_ppm.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED)
+#test_rc_ppm.CFLAGS += -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/ppm.h\"
+#test_rc_ppm.CFLAGS += -DRADIO_CONTROL_TYPE_PPM
+#test_rc_ppm.srcs += $(SRC_SUBSYSTEMS)/radio_control.c \
+# $(SRC_SUBSYSTEMS)/radio_control/ppm.c \
+# $(SRC_ARCH)/subsystems/radio_control/ppm_arch.c
+#test_rc_ppm.CFLAGS += -DUSE_TIM2_IRQ
+#
+##
+## test_adc
+##
+## configuration
+## SYS_TIME_LED
+## MODEM_PORT
+## MODEM_BAUD
+##
+#test_adc.ARCHDIR = $(ARCH)
+#test_adc.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
+#test_adc.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
+#
+#test_adc.srcs = $(SRC_AIRBORNE)/mcu.c \
+# $(SRC_ARCH)/mcu_arch.c \
+# $(SRC_LISA)/test_adc.c \
+# $(SRC_ARCH)/stm32_exceptions.c \
+# $(SRC_ARCH)/stm32_vector_table.c
+#
+#test_adc.CFLAGS += -DUSE_LED
+#test_adc.srcs += $(SRC_ARCH)/led_hw.c
+#
+#test_adc.CFLAGS += -DUSE_SYS_TIME
+#test_adc.CFLAGS +=-DSYS_TIME_LED=$(SYS_TIME_LED)
+#test_adc.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
+#test_adc.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+#
+#test_adc.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
+#test_adc.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
+#test_adc.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=$(MODEM_PORT)
+#
+#test_adc.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
+#test_adc.srcs += downlink.c pprz_transport.c
+#
+#test_adc.srcs += $(SRC_ARCH)/mcu_periph/adc_arch.c
+#test_adc.CFLAGS += -DUSE_AD1 -DUSE_AD1_1 -DUSE_AD1_2 -DUSE_AD1_3 -DUSE_AD1_4
+#test_adc.CFLAGS += -DUSE_ADC1_2_IRQ_HANDLER
+#
+##
+## test IMU b2
+##
+## configuration
+## SYS_TIME_LED
+## MODEM_PORT
+## MODEM_BAUD
+##
+#test_imu_b2.ARCHDIR = $(ARCH)
+#test_imu_b2.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT
+#test_imu_b2.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
+#test_imu_b2.srcs += $(SRC_AIRBORNE)/mcu.c \
+# $(SRC_ARCH)/mcu_arch.c \
+# $(SRC_BOOZ_TEST)/booz_test_imu.c \
+# $(SRC_ARCH)/stm32_exceptions.c \
+# $(SRC_ARCH)/stm32_vector_table.c
+#
+#test_imu_b2.CFLAGS += -DUSE_LED
+#test_imu_b2.srcs += $(SRC_ARCH)/led_hw.c
+#
+#test_imu_b2.CFLAGS += -DUSE_SYS_TIME
+#test_imu_b2.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED)
+#test_imu_b2.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))'
+#test_imu_b2.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+#
+#test_imu_b2.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
+#test_imu_b2.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
+#
+#test_imu_b2.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
+#test_imu_b2.srcs += downlink.c pprz_transport.c
+#
+#test_imu_b2.srcs += math/pprz_trig_int.c
+#
+#test_imu_b2.CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_b2.h\"
+#test_imu_b2.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_MS2001 -DIMU_B2_VERSION_1_1
+#test_imu_b2.srcs += $(SRC_SUBSYSTEMS)/imu.c
+#test_imu_b2.CFLAGS += -DMAX_1168_DRDY_PORT=$(MAX_1168_DRDY_PORT)
+#test_imu_b2.CFLAGS += -DMAX_1168_DRDY_PORT_SOURCE=$(MAX_1168_DRDY_PORT_SOURCE)
+#test_imu_b2.CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ
+#test_imu_b2.srcs += $(SRC_SUBSYSTEMS)/imu/imu_b2.c $(SRC_ARCH)/subsystems/imu/imu_b2_arch.c
+#test_imu_b2.srcs += peripherals/max1168.c $(SRC_ARCH)/peripherals/max1168_arch.c
+#test_imu_b2.srcs += peripherals/ms2001.c $(SRC_ARCH)/peripherals/ms2001_arch.c
+#
+##
+## test IMU b2 1.2
+##
+## configuration
+## SYS_TIME_LED
+## MODEM_PORT
+## MODEM_BAUD
+##
+#test_imu_b2_2.ARCHDIR = $(ARCH)
+#test_imu_b2_2.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT
+#test_imu_b2_2.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
+#test_imu_b2_2.srcs += $(SRC_AIRBORNE)/mcu.c \
+# $(SRC_ARCH)/mcu_arch.c \
+# $(SRC_BOOZ_TEST)/booz_test_imu.c \
+# $(SRC_ARCH)/stm32_exceptions.c \
+# $(SRC_ARCH)/stm32_vector_table.c
+#
+#test_imu_b2_2.CFLAGS += -DUSE_LED
+#test_imu_b2_2.srcs += $(SRC_ARCH)/led_hw.c
+#
+#test_imu_b2_2.CFLAGS += -DUSE_SYS_TIME
+#test_imu_b2_2.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED)
+#test_imu_b2_2.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))'
+#test_imu_b2_2.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+#
+#test_imu_b2_2.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
+#test_imu_b2_2.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
+#
+#test_imu_b2_2.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
+#test_imu_b2_2.srcs += downlink.c pprz_transport.c
+#
+#test_imu_b2_2.srcs += math/pprz_trig_int.c
+#
+#test_imu_b2_2.CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_b2.h\"
+#test_imu_b2_2.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_HMC5843 -DIMU_B2_VERSION_1_2
+#test_imu_b2_2.srcs += $(SRC_SUBSYSTEMS)/imu.c
+#test_imu_b2_2.CFLAGS += -DMAX_1168_DRDY_PORT=$(MAX_1168_DRDY_PORT)
+#test_imu_b2_2.CFLAGS += -DMAX_1168_DRDY_PORT_SOURCE=$(MAX_1168_DRDY_PORT_SOURCE)
+#test_imu_b2_2.CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ
+#test_imu_b2_2.srcs += $(SRC_SUBSYSTEMS)/imu/imu_b2.c $(SRC_ARCH)/subsystems/imu/imu_b2_arch.c
+#test_imu_b2_2.srcs += peripherals/max1168.c $(SRC_ARCH)/peripherals/max1168_arch.c
+#test_imu_b2_2.CFLAGS += -DUSE_I2C2
+#test_imu_b2_2.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c
+#test_imu_b2_2.srcs += peripherals/hmc5843.c $(SRC_ARCH)/peripherals/hmc5843_arch.c
+#test_imu_b2_2.CFLAGS += -DUSE_EXTI9_5_IRQ # Mag Int on PB5
+#
+#
+##
+## test_imu_aspirin : test aspirin imu
+##
+## configuration
+## MODEM_PORT :
+## MODEM_BAUD :
+##
+test_imu_aspirin.ARCHDIR = $(ARCH)
+test_imu_aspirin.CFLAGS += -I$(SRC_LISA) -I$(SRC_ARCH) -DPERIPHERALS_AUTO_INIT
+test_imu_aspirin.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
+test_imu_aspirin.srcs = $(SRC_AIRBORNE)/mcu.c \
+ $(SRC_ARCH)/mcu_arch.c \
+ $(SRC_ARCH)/stm32_exceptions.c \
+ $(SRC_ARCH)/stm32_vector_table.c\
+ booz/test/booz_test_imu.c
+
+test_imu_aspirin.CFLAGS += -DUSE_LED
+test_imu_aspirin.srcs += $(SRC_ARCH)/led_hw.c
+test_imu_aspirin.CFLAGS += -DUSE_SYS_TIME
+test_imu_aspirin.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
+test_imu_aspirin.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED)
+test_imu_aspirin.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+test_imu_aspirin.CFLAGS += -DUSE_$(MODEM_PORT)
+test_imu_aspirin.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
+test_imu_aspirin.srcs += downlink.c pprz_transport.c
+test_imu_aspirin.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
+test_imu_aspirin.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
+test_imu_aspirin.srcs += math/pprz_trig_int.c
+test_imu_aspirin.CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin.h\" -DIMU_OVERRIDE_CHANNELS
+test_imu_aspirin.srcs += $(SRC_SUBSYSTEMS)/imu.c \
+ $(SRC_SUBSYSTEMS)/imu/imu_aspirin.c \
+ $(SRC_ARCH)/subsystems/imu/imu_aspirin_arch.c
+test_imu_aspirin.srcs += peripherals/hmc5843.c $(SRC_ARCH)/peripherals/hmc5843_arch.c
+
+test_imu_aspirin.CFLAGS += -DUSE_I2C2
+test_imu_aspirin.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c
+test_imu_aspirin.CFLAGS += -DUSE_EXTI15_10_IRQ # Gyro Int on PC14
+test_imu_aspirin.CFLAGS += -DUSE_EXTI9_5_IRQ # Mag Int on PB5
+test_imu_aspirin.CFLAGS += -DUSE_EXTI2_IRQ # Accel Int on PD2
+test_imu_aspirin.CFLAGS += -DUSE_DMA1_C4_IRQ # SPI2 Rx DMA
+#
+##
+## test hmc5843
+##
+#test_hmc5843.ARCHDIR = $(ARCH)
+#test_hmc5843.CFLAGS = -I$(SRC_FIRMWARE) -I$(SRC_LISA) -I$(ARCH) -Ibooz -DPERIPHERALS_AUTO_INIT
+#test_hmc5843.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
+#test_hmc5843.srcs = $(SRC_AIRBORNE)/mcu.c \
+# $(SRC_ARCH)/mcu_arch.c \
+# lisa/test/lisa_test_hmc5843.c \
+# $(SRC_ARCH)/stm32_exceptions.c \
+# $(SRC_ARCH)/stm32_vector_table.c
+#test_hmc5843.CFLAGS += -DUSE_LED
+#test_hmc5843.srcs += $(SRC_ARCH)/led_hw.c
+#test_hmc5843.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED)
+#test_hmc5843.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
+#test_hmc5843.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+#
+#test_hmc5843.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
+#test_hmc5843.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
+#
+#test_hmc5843.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
+#test_hmc5843.srcs += downlink.c pprz_transport.c
+#
+#test_hmc5843.CFLAGS += -DUSE_I2C2
+#test_hmc5843.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c
+#test_hmc5843.CFLAGS += -DIMU_OVERRIDE_CHANNELS
+#test_hmc5843.CFLAGS += -DUSE_EXTI9_5_IRQ # Mag Int on PB5
+#
+#
+##
+## test ITG3200
+##
+#test_itg3200.ARCHDIR = $(ARCH)
+#test_itg3200.CFLAGS = -I$(SRC_FIRMWARE) -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT
+#test_itg3200.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
+#test_itg3200.srcs += $(SRC_AIRBORNE)/mcu.c \
+# $(SRC_ARCH)/mcu_arch.c \
+# lisa/test/lisa_test_itg3200.c \
+# $(SRC_ARCH)/stm32_exceptions.c \
+# $(SRC_ARCH)/stm32_vector_table.c
+#
+#test_itg3200.CFLAGS += -DUSE_LED
+#test_itg3200.srcs += $(SRC_ARCH)/led_hw.c
+#
+#test_itg3200.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED)
+#test_itg3200.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))'
+#test_itg3200.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+#
+#test_itg3200.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
+#test_itg3200.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
+#
+#test_itg3200.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
+#test_itg3200.srcs += downlink.c pprz_transport.c
+#
+#test_itg3200.CFLAGS += -DUSE_I2C2
+#test_itg3200.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c
+#test_itg3200.CFLAGS += -DUSE_EXTI15_10_IRQ # Gyro Int on PC14
+#
+#
+##
+## test adxl345 with DMA
+##
+#test_adxl345.ARCHDIR = $(ARCH)
+#test_adxl345.CFLAGS = -I$(SRC_FIRMWARE) -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT
+#test_adxl345.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
+#test_adxl345.srcs += $(SRC_AIRBORNE)/mcu.c \
+# $(SRC_ARCH)/mcu_arch.c \
+# lisa/test/lisa_test_adxl345_dma.c \
+# $(SRC_ARCH)/stm32_exceptions.c \
+# $(SRC_ARCH)/stm32_vector_table.c
+#
+#test_adxl345.CFLAGS += -DUSE_LED
+#test_adxl345.srcs += $(SRC_ARCH)/led_hw.c
+#
+#test_adxl345.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
+#test_adxl345.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))'
+#test_adxl345.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+#
+#test_adxl345.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
+#test_adxl345.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
+#
+#test_adxl345.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
+#test_adxl345.srcs += downlink.c pprz_transport.c
+#
+#test_adxl345.CFLAGS += -DUSE_EXTI2_IRQ # Accel Int on PD2
+#test_adxl345.CFLAGS += -DUSE_DMA1_C4_IRQ # SPI2 Rx DMA
+#
+#
+#
+##
+## simple test of mikrokopter motor controllers
+##
+#test_esc_mkk_simple.ARCHDIR = $(ARCH)
+#test_esc_mkk_simple.CFLAGS = -I$(SRC_FIRMWARE) -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
+#test_esc_mkk_simple.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
+#test_esc_mkk_simple.srcs = $(SRC_AIRBORNE)/mcu.c \
+# $(SRC_ARCH)/mcu_arch.c \
+# test/test_esc_mkk_simple.c \
+# $(SRC_ARCH)/stm32_exceptions.c \
+# $(SRC_ARCH)/stm32_vector_table.c
+#test_esc_mkk_simple.CFLAGS += -DUSE_LED
+#test_esc_mkk_simple.srcs += $(SRC_ARCH)/led_hw.c
+#test_esc_mkk_simple.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED)
+#test_esc_mkk_simple.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
+#test_esc_mkk_simple.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+#test_esc_mkk_simple.CFLAGS += -DUSE_I2C2
+#test_esc_mkk_simple.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c
+#test_esc_mkk_simple.CFLAGS += -DACTUATORS_MKK_DEV=i2c2
+#
+#
+##
+## simple test of asctec v1 motor controllers
+##
+#test_esc_asctecv1_simple.ARCHDIR = $(ARCH)
+#test_esc_asctecv1_simple.CFLAGS = -I$(SRC_FIRMWARE) -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
+#test_esc_asctecv1_simple.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
+#test_esc_asctecv1_simple.srcs = $(SRC_AIRBORNE)/mcu.c \
+# $(SRC_ARCH)/mcu_arch.c \
+# test/test_esc_asctecv1_simple.c \
+# $(SRC_ARCH)/stm32_exceptions.c \
+# $(SRC_ARCH)/stm32_vector_table.c
+#test_esc_asctecv1_simple.CFLAGS += -DUSE_LED
+#test_esc_asctecv1_simple.srcs += $(SRC_ARCH)/led_hw.c
+#test_esc_asctecv1_simple.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
+#test_esc_asctecv1_simple.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
+#test_esc_asctecv1_simple.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+#test_esc_asctecv1_simple.CFLAGS += -DUSE_I2C1
+#test_esc_asctecv1_simple.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c
+#
+#
+##
+## test actuators mkk
+##
+#test_actuators_mkk.ARCHDIR = $(ARCH)
+#test_actuators_mkk.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ) -DPERIPHERALS_AUTO_INIT
+#test_actuators_mkk.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
+#test_actuators_mkk.srcs = $(SRC_AIRBORNE)/mcu.c \
+# $(SRC_ARCH)/mcu_arch.c \
+# test/test_actuators.c \
+# $(SRC_ARCH)/stm32_exceptions.c \
+# $(SRC_ARCH)/stm32_vector_table.c
+#
+#test_actuators_mkk.CFLAGS += -DUSE_LED
+#test_actuators_mkk.srcs += $(SRC_ARCH)/led_hw.c
+#
+#test_actuators_mkk.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
+#test_actuators_mkk.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
+#test_actuators_mkk.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+#
+#test_actuators_mkk.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
+#test_actuators_mkk.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
+#
+#test_actuators_mkk.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
+#test_actuators_mkk.srcs += downlink.c pprz_transport.c
+#
+#test_actuators_mkk.srcs += $(SRC_BOOZ)/booz2_commands.c
+#test_actuators_mkk.srcs += $(SRC_FIRMWARE)/actuators/actuators_mkk.c
+#test_actuators_mkk.CFLAGS += -DACTUATORS_MKK_DEVICE=i2c1
+#test_actuators_mkk.srcs += $(SRC_FIRMWARE)/actuators/supervision.c
+#test_actuators_mkk.CFLAGS += -DUSE_I2C1
+#test_actuators_mkk.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c
+#
+##
+## test actuators asctecv1
+##
+#test_actuators_asctecv1.ARCHDIR = $(ARCH)
+#test_actuators_asctecv1.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ) -DPERIPHERALS_AUTO_INIT
+#test_actuators_asctecv1.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
+#test_actuators_asctecv1.srcs = $(SRC_AIRBORNE)/mcu.c \
+# $(SRC_ARCH)/mcu_arch.c \
+# test/test_actuators.c \
+# $(SRC_ARCH)/stm32_exceptions.c \
+# $(SRC_ARCH)/stm32_vector_table.c
+#
+#test_actuators_asctecv1.CFLAGS += -DUSE_LED
+#test_actuators_asctecv1.srcs += $(SRC_ARCH)/led_hw.c
+#
+#test_actuators_asctecv1.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
+#test_actuators_asctecv1.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
+#test_actuators_asctecv1.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+#
+#test_actuators_asctecv1.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
+#test_actuators_asctecv1.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
+#
+#test_actuators_asctecv1.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
+#test_actuators_asctecv1.srcs += downlink.c pprz_transport.c
+#
+#test_actuators_asctecv1.srcs += $(SRC_BOOZ)/booz2_commands.c
+#test_actuators_asctecv1.CFLAGS += -DACTUATORS_ASCTEC_DEVICE=i2c1
+#test_actuators_asctecv1.srcs += $(SRC_FIRMWARE)/actuators/actuators_asctec.c
+#test_actuators_asctecv1.CFLAGS += -DUSE_I2C1
+#test_actuators_asctecv1.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c
+#
+#
+##
+## test bmp085
+##
+#test_bmp085.ARCHDIR = $(ARCH)
+#test_bmp085.CFLAGS = -I$(SRC_FIRMWARE) -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
+#test_bmp085.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
+#test_bmp085.srcs = $(SRC_AIRBORNE)/mcu.c \
+# $(SRC_ARCH)/mcu_arch.c \
+# lisa/test/lisa_test_bmp085.c \
+# $(SRC_ARCH)/stm32_exceptions.c \
+# $(SRC_ARCH)/stm32_vector_table.c
+#test_bmp085.CFLAGS += -DUSE_LED
+#test_bmp085.srcs += $(SRC_ARCH)/led_hw.c
+#test_bmp085.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED)
+#test_bmp085.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
+#test_bmp085.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+#
+#test_bmp085.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
+#test_bmp085.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
+#
+#test_bmp085.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
+#test_bmp085.srcs += downlink.c pprz_transport.c
+#
+#test_bmp085.CFLAGS += -DUSE_I2C2
+#test_bmp085.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c
+##test_bmp085.CFLAGS += -DIMU_OVERRIDE_CHANNELS
+##test_bmp085.CFLAGS += -DUSE_EXTI9_5_IRQ # Mag Int on PB5
+#
+#
+#
+##
+## Test manual : a simple test with rc and servos - I want to fly lisa/M
+##
+#test_manual.ARCHDIR = $(ARCH)
+#test_manual.CFLAGS = -I$(SRC_FIRMWARE) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
+#test_manual.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
+#test_manual.srcs = $(SRC_AIRBORNE)/mcu.c \
+# $(SRC_ARCH)/mcu_arch.c \
+# test/test_manual.c \
+# $(SRC_ARCH)/stm32_exceptions.c \
+# $(SRC_ARCH)/stm32_vector_table.c
+#test_manual.CFLAGS += -DUSE_LED
+#test_manual.srcs += $(SRC_ARCH)/led_hw.c
+#test_manual.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED)
+#test_manual.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
+#test_manual.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+#
+#test_manual.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
+#test_manual.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
+#
+#test_manual.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
+#test_manual.srcs += downlink.c pprz_transport.c
+#
+#test_manual.srcs += $(SRC_BOOZ)/booz2_commands.c
+#
+#test_manual.CFLAGS += -I$(SRC_FIRMWARE)/actuators/arch/$(ARCH)
+##test_manual.srcs += $(SRC_FIRMWARE)/actuators/actuators_pwm.c
+#test_manual.srcs += $(SRC_FIRMWARE)/actuators/arch/$(ARCH)/actuators_pwm_arch.c
+#test_manual.srcs += $(SRC_FIRMWARE)/actuators/actuators_heli.c
+#
+#
+#test_manual.CFLAGS += -I$(SRC_BOOZ) -I$(SRC_BOOZ)/arch/$(ARCH)
+#test_manual.CFLAGS += -DRADIO_CONTROL
+#ifdef RADIO_CONTROL_LED
+#test_manual.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED)
+#endif
+#test_manual.CFLAGS += -DRADIO_CONTROL_BIND_IMPL_FUNC=radio_control_spektrum_try_bind
+#test_manual.CFLAGS += -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/spektrum.h\"
+#test_manual.CFLAGS += -DRADIO_CONTROL_SPEKTRUM_PRIMARY_PORT=$(RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT)
+#test_manual.CFLAGS += -DOVERRIDE_$(RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT)_IRQ_HANDLER -DUSE_TIM6_IRQ
+#test_manual.srcs += $(SRC_SUBSYSTEMS)/radio_control.c \
+# subsystems/radio_control/spektrum.c \
+# $(SRC_ARCH)/subsystems/radio_control/spektrum_arch.c
+#
+#
+#
+##
+## tunnel
+##
+#tunnel.ARCHDIR = $(ARCH)
+#tunnel.CFLAGS += -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
+#tunnel.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
+#tunnel.srcs += $(SRC_AIRBORNE)/mcu.c \
+# $(SRC_ARCH)/mcu_arch.c \
+# $(SRC_LISA)/tunnel_hw.c \
+# $(SRC_ARCH)/stm32_exceptions.c \
+# $(SRC_ARCH)/stm32_vector_table.c
+#tunnel.CFLAGS += -DUSE_LED
+#tunnel.srcs += $(SRC_ARCH)/led_hw.c
+#tunnel.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED)
+#tunnel.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
+#tunnel.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
diff --git a/conf/autopilot/lisa_m_test_progs.xml b/conf/autopilot/lisa_m_test_progs.xml
new file mode 100644
index 0000000000..5a7712caed
--- /dev/null
+++ b/conf/autopilot/lisa_m_test_progs.xml
@@ -0,0 +1,25 @@
+
+
+
+
+
diff --git a/conf/autopilot/obsolete/lisa_test_progs.makefile b/conf/autopilot/obsolete/lisa_test_progs.makefile
index 7be32e25e9..1a48c121c4 100644
--- a/conf/autopilot/obsolete/lisa_test_progs.makefile
+++ b/conf/autopilot/obsolete/lisa_test_progs.makefile
@@ -424,12 +424,12 @@ test_imu_b2.srcs += downlink.c pprz_transport.c
test_imu_b2.srcs += math/pprz_trig_int.c
test_imu_b2.CFLAGS += -DIMU_TYPE_H=\"imu/imu_b2.h\"
-test_imu_b2.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_MS2001
+test_imu_b2.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_MS2100
test_imu_b2.srcs += $(SRC_SUBSYSTEMS)/imu.c
test_imu_b2.CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ
test_imu_b2.srcs += $(SRC_SUBSYSTEMS)/imu/imu_b2.c $(SRC_SUBSYSTEMS)/imu/arch/$(ARCH)/imu_b2_arch.c
test_imu_b2.srcs += peripherals/max1168.c $(SRC_ARCH)/peripherals/max1168_arch.c
-test_imu_b2.srcs += peripherals/ms2001.c $(SRC_ARCH)/peripherals/ms2001_arch.c
+test_imu_b2.srcs += peripherals/ms2100.c $(SRC_ARCH)/peripherals/ms2100_arch.c
#
@@ -884,33 +884,33 @@ test_max1168.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_
test_max1168.srcs += downlink.c pprz_transport.c
#
-# test ms2001
+# test ms2100
#
-test_ms2001.ARCHDIR = $(ARCH)
-test_ms2001.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT
-test_ms2001.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_ms2001.srcs = $(SRC_LISA)/test/lisa_test_ms2001.c \
+test_ms2100.ARCHDIR = $(ARCH)
+test_ms2100.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT
+test_ms2100.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
+test_ms2100.srcs = $(SRC_LISA)/test/lisa_test_ms2100.c \
$(SRC_ARCH)/stm32_exceptions.c \
$(SRC_ARCH)/stm32_vector_table.c
-test_ms2001.CFLAGS += -DUSE_LED
-test_ms2001.srcs += $(SRC_ARCH)/led_hw.c
+test_ms2100.CFLAGS += -DUSE_LED
+test_ms2100.srcs += $(SRC_ARCH)/led_hw.c
-test_ms2001.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_ms2001.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-test_ms2001.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+test_ms2100.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
+test_ms2100.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
+test_ms2100.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-test_ms2001.CFLAGS += -DUSE_SPI2
-test_ms2001.CFLAGS += -DUSE_DMA1_C4_IRQ -DMS2001_HANDLES_DMA_IRQ
-test_ms2001.CFLAGS += -DUSE_SPI2_IRQ -DMS2001_HANDLES_SPI_IRQ
-test_ms2001.srcs += peripherals/ms2001.c \
- $(SRC_ARCH)/peripherals/ms2001_arch.c
+test_ms2100.CFLAGS += -DUSE_SPI2
+test_ms2100.CFLAGS += -DUSE_DMA1_C4_IRQ -DMS2100_HANDLES_DMA_IRQ
+test_ms2100.CFLAGS += -DUSE_SPI2_IRQ -DMS2100_HANDLES_SPI_IRQ
+test_ms2100.srcs += peripherals/ms2100.c \
+ $(SRC_ARCH)/peripherals/ms2100_arch.c
-test_ms2001.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
-test_ms2001.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
+test_ms2100.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
+test_ms2100.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-test_ms2001.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
-test_ms2001.srcs += downlink.c pprz_transport.c
+test_ms2100.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
+test_ms2100.srcs += downlink.c pprz_transport.c
#
# test adxl345
@@ -1092,12 +1092,12 @@ ptw.srcs += downlink.c pprz_transport.c
# IMU
ptw.CFLAGS += -DIMU_TYPE_H=\"imu/imu_b2.h\"
-ptw.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_MS2001
+ptw.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_MS2100
ptw.srcs += $(SRC_SUBSYSTEMS)/imu.c
ptw.CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ
ptw.srcs += $(SRC_SUBSYSTEMS)/imu/imu_b2.c $(SRC_SUBSYSTEMS)/imu/arch/$(ARCH)/imu_b2_arch.c
ptw.srcs += peripherals/max1168.c $(SRC_ARCH)/peripherals/max1168_arch.c
-ptw.srcs += peripherals/ms2001.c $(SRC_ARCH)/peripherals/ms2001_arch.c
+ptw.srcs += peripherals/ms2100.c $(SRC_ARCH)/peripherals/ms2100_arch.c
ptw.srcs += math/pprz_trig_int.c
ptw.srcs += $(SRC_BOOZ)/booz2_commands.c
@@ -1351,12 +1351,12 @@ hs_gyro_b2.srcs += downlink.c pprz_transport.c
hs_gyro_b2.srcs += math/pprz_trig_int.c
hs_gyro_b2.CFLAGS += -DIMU_TYPE_H=\"imu/imu_b2.h\"
-hs_gyro_b2.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_MS2001
+hs_gyro_b2.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_MS2100
hs_gyro_b2.srcs += $(SRC_SUBSYSTEMS)/imu.c
hs_gyro_b2.CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ
hs_gyro_b2.srcs += $(SRC_SUBSYSTEMS)/imu/imu_b2.c $(SRC_SUBSYSTEMS)/imu/arch/$(ARCH)/imu_b2_arch.c
hs_gyro_b2.srcs += peripherals/max1168.c $(SRC_ARCH)/peripherals/max1168_arch.c
-hs_gyro_b2.srcs += peripherals/ms2001.c $(SRC_ARCH)/peripherals/ms2001_arch.c
+hs_gyro_b2.srcs += peripherals/ms2100.c $(SRC_ARCH)/peripherals/ms2100_arch.c
#
# Spits every samples of one axis of gyro on IMU crista
diff --git a/conf/autopilot/rotorcraft.makefile b/conf/autopilot/rotorcraft.makefile
index 7ac8e8e763..d69f1c75b4 100644
--- a/conf/autopilot/rotorcraft.makefile
+++ b/conf/autopilot/rotorcraft.makefile
@@ -93,9 +93,15 @@ endif
# or
# include subsystems/rotorcraft/telemetry_xbee_api.makefile
#
+ap.srcs += subsystems/settings.c
+ap.srcs += $(SRC_ARCH)/subsystems/settings_arch.c
+
ap.srcs += mcu_periph/uart.c
ap.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
+# I2C is needed for speed controllers and barometers on lisa
+ap.srcs += mcu_periph/i2c.c
+ap.srcs += $(SRC_ARCH)/mcu_periph/i2c_arch.c
ap.srcs += $(SRC_BOOZ)/booz2_commands.c
@@ -132,7 +138,7 @@ ap.srcs += $(SRC_BOOZ)/booz2_commands.c
#
ap.srcs += $(SRC_BOARD)/baro_board.c
ifeq ($(BOARD), booz)
-ap.CFLAGS += -DROTORCRAFT_BARO_LED=$(BARO_LED) -DBOOZ2_ANALOG_BARO_PERIOD='SYS_TICS_OF_SEC((1./100.))'
+ap.CFLAGS += -DROTORCRAFT_BARO_LED=$(BARO_LED)
else ifeq ($(BOARD), lisa_l)
ap.CFLAGS += -DUSE_I2C2
endif
@@ -140,18 +146,26 @@ endif
#
# Analog Backend
#
+
ifeq ($(ARCH), lpc21)
-ap.CFLAGS += -DBOOZ2_ANALOG_BATTERY_PERIOD='SYS_TICS_OF_SEC((1./10.))'
-ap.srcs += $(SRC_FIRMWARE)/battery.c
ap.CFLAGS += -DADC0_VIC_SLOT=2
ap.CFLAGS += -DADC1_VIC_SLOT=3
-ap.srcs += $(SRC_BOOZ)/booz2_analog.c \
- $(SRC_BOOZ_ARCH)/booz2_analog_hw.c
+ap.CFLAGS += -DUSE_ADC
+ap.srcs += $(SRC_ARCH)/mcu_periph/adc_arch.c
+ap.srcs += subsystems/electrical.c
+# baro has variable offset amplifier on booz board
+ap.CFLAGS += -DUSE_DAC
+ap.srcs += $(SRC_ARCH)/mcu_periph/dac_arch.c
else ifeq ($(ARCH), stm32)
-ap.srcs += lisa/lisa_analog_plug.c
+ap.CFLAGS += -DUSE_ADC
+ap.CFLAGS += -DUSE_AD1 -DUSE_AD1_1 -DUSE_AD1_2 -DUSE_AD1_3 -DUSE_AD1_4
+ap.CFLAGS += -DUSE_ADC1_2_IRQ_HANDLER
+ap.srcs += $(SRC_ARCH)/mcu_periph/adc_arch.c
+ap.srcs += subsystems/electrical.c
endif
+
#
# GPS choice
#
diff --git a/conf/autopilot/rotorcraft.xml b/conf/autopilot/rotorcraft.xml
index a28c88f984..93467a93ff 100644
--- a/conf/autopilot/rotorcraft.xml
+++ b/conf/autopilot/rotorcraft.xml
@@ -18,8 +18,8 @@
-
-
+
+
diff --git a/conf/autopilot/setup.makefile b/conf/autopilot/setup.makefile
index b71b547167..bd1757dbb9 100644
--- a/conf/autopilot/setup.makefile
+++ b/conf/autopilot/setup.makefile
@@ -23,6 +23,7 @@ tunnel.srcs += mcu.c $(SRC_ARCH)/mcu_arch.c
# for the usb_tunnel we need to set PCLK higher with the flag USE_USB_HIGH_PCLK
# a configuration program to access both uart through usb
+ifeq ($(ARCH), lpc21)
usb_tunnel_0.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B115200 -DPERIPHERALS_AUTO_INIT
usb_tunnel_0.CFLAGS += -DUSE_USB_LINE_CODING -DUSE_USB_SERIAL -DUSE_LED -DUSE_USB_HIGH_PCLK
usb_tunnel_0.srcs += $(SRC_ARCH)/usb_tunnel.c $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/mcu_periph/uart_arch.c
@@ -38,6 +39,9 @@ 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 += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
usb_tunnel_1.srcs += mcu.c $(SRC_ARCH)/mcu_arch.c
+else
+$(error usb_tunnel currently only implemented for the lpc21)
+endif
diff --git a/conf/autopilot/subsystems/fixedwing/autopilot.makefile b/conf/autopilot/subsystems/fixedwing/autopilot.makefile
index 5fda288b9b..a301d3c58a 100644
--- a/conf/autopilot/subsystems/fixedwing/autopilot.makefile
+++ b/conf/autopilot/subsystems/fixedwing/autopilot.makefile
@@ -134,6 +134,8 @@ ns_srcs += $(SRC_ARCH)/sys_time_hw.c
ns_srcs += mcu_periph/uart.c
ns_srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
+ns_srcs += subsystems/settings.c
+ns_srcs += $(SRC_ARCH)/subsystems/settings_arch.c
#
# ANALOG
@@ -143,6 +145,7 @@ ns_srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
#ifeq ($(ARCH), lpc21)
ns_srcs += $(SRC_ARCH)/mcu_periph/adc_arch.c
ifeq ($(ARCH), stm32)
+ ns_CFLAGS += -DUSE_AD1 -DUSE_AD1_1 -DUSE_AD1_2 -DUSE_AD1_3 -DUSE_AD1_4
ns_CFLAGS += -DUSE_ADC1_2_IRQ_HANDLER
endif
@@ -186,11 +189,15 @@ sim.srcs += $(SRC_ARCH)/sim_ap.c
sim.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport
sim.srcs += downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c
+sim.srcs += subsystems/settings.c
+sim.srcs += $(SRC_ARCH)/subsystems/settings_arch.c
+
######################################################################
##
## JSBSIM THREAD SPECIFIC
##
+OCAMLLIBDIR=$(shell ocamlc -where)
JSBSIM_INC = /usr/include/JSBSim
#JSBSIM_LIB = /usr/lib
@@ -198,10 +205,10 @@ jsbsim.CFLAGS += $(fbw_CFLAGS) $(ap_CFLAGS)
jsbsim.srcs += $(fbw_srcs) $(ap_srcs)
jsbsim.CFLAGS += -DSITL
-jsbsim.srcs += $(SIMDIR)/sim_ac_jsbsim.c $(SIMDIR)/sim_ac_fw.c
+jsbsim.srcs += $(SIMDIR)/sim_ac_jsbsim.c $(SIMDIR)/sim_ac_fw.c $(SIMDIR)/sim_ac_flightgear.c
# external libraries
-jsbsim.CFLAGS += -I$(SIMDIR) -I/usr/include -I$(JSBSIM_INC) `pkg-config glib-2.0 --cflags`
+jsbsim.CFLAGS += -I$(SIMDIR) -I/usr/include -I$(JSBSIM_INC) -I$(OCAMLLIBDIR) `pkg-config glib-2.0 --cflags`
jsbsim.LDFLAGS += `pkg-config glib-2.0 --libs` -lm -lpcre -lglibivy -L/usr/lib -lJSBSim
jsbsim.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport
diff --git a/conf/autopilot/subsystems/fixedwing/control_new.makefile b/conf/autopilot/subsystems/fixedwing/control_new.makefile
index 267b9eff8f..db21505cbc 100644
--- a/conf/autopilot/subsystems/fixedwing/control_new.makefile
+++ b/conf/autopilot/subsystems/fixedwing/control_new.makefile
@@ -1,4 +1,4 @@
# new fixed wing control loops with merged auto pitch and auto throttle, adaptive horizontal control
+$(TARGET).srcs += $(SRC_FIRMWARE)/stabilization/stabilization_adaptive.c $(SRC_FIRMWARE)/guidance/guidance_v_n.c
-$(TARGET).srcs += $(SRC_FIXEDWING)/fw_h_ctl_a.c $(SRC_FIXEDWING)/fw_v_ctl_n.c
diff --git a/conf/autopilot/subsystems/fixedwing/imu_booz.makefile b/conf/autopilot/subsystems/fixedwing/imu_booz.makefile
index e4d397d37a..e6ad633f17 100644
--- a/conf/autopilot/subsystems/fixedwing/imu_booz.makefile
+++ b/conf/autopilot/subsystems/fixedwing/imu_booz.makefile
@@ -52,7 +52,7 @@ imu_srcs += $(SRC_ARCH)/peripherals/max1168_arch.c
#ifeq ($(ARCH), lpc21)
imu_CFLAGS += -DSSP_VIC_SLOT=9
imu_CFLAGS += -DMAX1168_EOC_VIC_SLOT=8
-#FIXME ms2001 not used on this imu
+#FIXME ms2100 not used on this imu
#else ifeq ($(ARCH), stm32)
#imu_CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ
#imu_CFLAGS += -DMAX_1168_DRDY_PORT=$(MAX_1168_DRDY_PORT)
diff --git a/conf/autopilot/subsystems/fixedwing/navigation_extra.makefile b/conf/autopilot/subsystems/fixedwing/navigation_extra.makefile
index 91be2eba88..60cc01c1af 100644
--- a/conf/autopilot/subsystems/fixedwing/navigation_extra.makefile
+++ b/conf/autopilot/subsystems/fixedwing/navigation_extra.makefile
@@ -12,4 +12,6 @@ $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/nav_cube.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/discsurvey.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/OSAMNav.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/snav.c
+$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/spiral.c
+$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/poly_survey_adv.c
diff --git a/conf/autopilot/subsystems/fixedwing/radio_control_datalink.makefile b/conf/autopilot/subsystems/fixedwing/radio_control_datalink.makefile
index e33a41c25e..dc36fad956 100644
--- a/conf/autopilot/subsystems/fixedwing/radio_control_datalink.makefile
+++ b/conf/autopilot/subsystems/fixedwing/radio_control_datalink.makefile
@@ -15,5 +15,5 @@ ifeq ($(NORADIO), False)
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/radio_control.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/radio_control/rc_datalink.c
# arch only with sim target for compatibility (empty functions)
- sim.srcs += $(SRC_ARCH)/radio_control/rc_datalink.c
+ sim.srcs += $(SRC_ARCH)/subsystems/radio_control/rc_datalink.c
endif
diff --git a/conf/autopilot/subsystems/fixedwing/telemetry_transparent_usb.makefile b/conf/autopilot/subsystems/fixedwing/telemetry_transparent_usb.makefile
index fdc2d31baf..4221fce81d 100644
--- a/conf/autopilot/subsystems/fixedwing/telemetry_transparent_usb.makefile
+++ b/conf/autopilot/subsystems/fixedwing/telemetry_transparent_usb.makefile
@@ -1,9 +1,15 @@
#serial USB (e.g. /dev/ttyACM0)
+ifeq ($(ARCH), lpc21)
ap.CFLAGS += -DDOWNLINK -DDOWNLINK_FBW_DEVICE=UsbS -DDOWNLINK_AP_DEVICE=UsbS -DPPRZ_UART=UsbS
ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ -DUSE_USB_SERIAL -DUSE_USB_HIGH_PCLK
ap.srcs += $(SRC_FIXEDWING)/downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_FIXEDWING)/pprz_transport.c
ap.srcs += $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbcontrol.c
ap.srcs += $(SRC_ARCH)/lpcusb/usbstdreq.c $(SRC_ARCH)/lpcusb/usbinit.c
+else
+ifneq ($(ARCH), sim)
+$(error telemetry_transparent_usb currently only implemented for the lpc21)
+endif
+endif
diff --git a/conf/autopilot/subsystems/lisa_passthrough/ahrs_cmpl.makefile b/conf/autopilot/subsystems/lisa_passthrough/ahrs_cmpl.makefile
index f13e52aeba..423f48203e 100644
--- a/conf/autopilot/subsystems/lisa_passthrough/ahrs_cmpl.makefile
+++ b/conf/autopilot/subsystems/lisa_passthrough/ahrs_cmpl.makefile
@@ -2,7 +2,7 @@
# Complementary filter for attitude estimation
#
-ap.CFLAGS += -DUSE_AHRS_CMPL -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) -DAHRS_FIXED_POINT
+ap.CFLAGS += -DUSE_AHRS_CMPL -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
stm_passthrough.srcs += $(SRC_SUBSYSTEMS)/ahrs.c
stm_passthrough.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c
stm_passthrough.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_int_cmpl_euler.c
diff --git a/conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.1.makefile b/conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.1.makefile
index 546bde2264..6bb570edb2 100644
--- a/conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.1.makefile
+++ b/conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.1.makefile
@@ -42,7 +42,7 @@
# imu Booz2 v1.1
imu_CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_b2.h\"
-imu_CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_MS2001
+imu_CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_MS2100
imu_CFLAGS += -DIMU_B2_VERSION_1_1
imu_srcs += $(SRC_SUBSYSTEMS)/imu.c
imu_srcs += $(SRC_SUBSYSTEMS)/imu/imu_b2.c
@@ -51,13 +51,13 @@ imu_srcs += $(SRC_ARCH)/subsystems/imu/imu_b2_arch.c
imu_srcs += peripherals/max1168.c
imu_srcs += $(SRC_ARCH)/peripherals/max1168_arch.c
-imu_srcs += peripherals/ms2001.c
-imu_srcs += $(SRC_ARCH)/peripherals/ms2001_arch.c
+imu_srcs += peripherals/ms2100.c
+imu_srcs += $(SRC_ARCH)/peripherals/ms2100_arch.c
ifeq ($(ARCH), lpc21)
imu_CFLAGS += -DSSP_VIC_SLOT=9
imu_CFLAGS += -DMAX1168_EOC_VIC_SLOT=8
-imu_CFLAGS += -DMS2001_DRDY_VIC_SLOT=11
+imu_CFLAGS += -DMS2100_DRDY_VIC_SLOT=11
else ifeq ($(ARCH), stm32)
imu_CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ
imu_CFLAGS += -DMAX_1168_DRDY_PORT=$(MAX_1168_DRDY_PORT)
diff --git a/conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.2.makefile b/conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.2.makefile
index 844b6c786d..1e562fcc24 100644
--- a/conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.2.makefile
+++ b/conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.2.makefile
@@ -57,8 +57,8 @@ imu_srcs += $(SRC_ARCH)/peripherals/hmc5843_arch.c
ifeq ($(ARCH), lpc21)
imu_CFLAGS += -DSSP_VIC_SLOT=9
imu_CFLAGS += -DMAX1168_EOC_VIC_SLOT=8
-#FIXME ms2001 not used on this imu
-imu_CFLAGS += -DMS2001_DRDY_VIC_SLOT=11
+#FIXME ms2100 not used on this imu
+imu_CFLAGS += -DMS2100_DRDY_VIC_SLOT=11
else ifeq ($(ARCH), stm32)
imu_CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ
imu_CFLAGS += -DMAX_1168_DRDY_PORT=$(MAX_1168_DRDY_PORT)
diff --git a/conf/autopilot/subsystems/rotorcraft/actuators_asctec.makefile b/conf/autopilot/subsystems/rotorcraft/actuators_asctec.makefile
index a3c61afbef..e31987e894 100644
--- a/conf/autopilot/subsystems/rotorcraft/actuators_asctec.makefile
+++ b/conf/autopilot/subsystems/rotorcraft/actuators_asctec.makefile
@@ -1,7 +1,5 @@
# asctec controllers
ap.srcs += $(SRC_FIRMWARE)/actuators/actuators_asctec.c
-ap.srcs += mcu_periph/i2c.c
-ap.srcs += $(SRC_ARCH)/mcu_periph/i2c_arch.c
ifeq ($(ARCH), lpc21)
ap.CFLAGS += -DACTUATORS_ASCTEC_DEVICE=i2c0
@@ -16,7 +14,7 @@ endif
# Simulator
sim.srcs += $(SRC_FIRMWARE)/actuators/actuators_asctec.c
-sim.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=10
+sim.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=11
sim.srcs += mcu_periph/i2c.c
sim.srcs += $(SRC_ARCH)/mcu_periph/i2c_arch.c
diff --git a/conf/autopilot/subsystems/rotorcraft/actuators_asctec_v2.makefile b/conf/autopilot/subsystems/rotorcraft/actuators_asctec_v2.makefile
index c1cfc48f37..30451deebb 100644
--- a/conf/autopilot/subsystems/rotorcraft/actuators_asctec_v2.makefile
+++ b/conf/autopilot/subsystems/rotorcraft/actuators_asctec_v2.makefile
@@ -19,12 +19,10 @@
ap.srcs += $(SRC_FIRMWARE)/actuators/supervision.c
ap.CFLAGS += -DACTUATORS_ASCTEC_V2_PROTOCOL
ap.srcs += $(SRC_FIRMWARE)/actuators/actuators_asctec.c
-ap.srcs += mcu_periph/i2c.c
-ap.srcs += $(SRC_ARCH)/mcu_periph/i2c_arch.c
ifeq ($(ARCH), lpc21)
ap.CFLAGS += -DACTUATORS_ASCTEC_DEVICE=i2c0
-ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=10
+ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=11
endif
ifeq ($(ARCH), stm32)
diff --git a/conf/autopilot/subsystems/rotorcraft/actuators_dummy.makefile b/conf/autopilot/subsystems/rotorcraft/actuators_dummy.makefile
new file mode 100644
index 0000000000..5c6b93839a
--- /dev/null
+++ b/conf/autopilot/subsystems/rotorcraft/actuators_dummy.makefile
@@ -0,0 +1,5 @@
+#
+# empty dummy actuators for testing
+#
+
+ap.srcs += $(SRC_FIRMWARE)/actuators/actuators_dummy.c
diff --git a/conf/autopilot/subsystems/rotorcraft/actuators_heli.makefile b/conf/autopilot/subsystems/rotorcraft/actuators_heli.makefile
index 95e3591951..d89410102e 100644
--- a/conf/autopilot/subsystems/rotorcraft/actuators_heli.makefile
+++ b/conf/autopilot/subsystems/rotorcraft/actuators_heli.makefile
@@ -5,6 +5,3 @@ ap.CFLAGS += -DUSE_HELI
ap.srcs += $(SRC_FIRMWARE)/actuators/actuators_heli.c
ap.srcs += $(SRC_FIRMWARE)/actuators/arch/$(ARCH)/actuators_pwm_arch.c
-
-# fixme : this is needed by baro and usualy added by actuators_mkk or actuators_asctec
-ap.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c
diff --git a/conf/autopilot/subsystems/rotorcraft/actuators_mkk.makefile b/conf/autopilot/subsystems/rotorcraft/actuators_mkk.makefile
index 3ea53af26e..a55d0e4313 100644
--- a/conf/autopilot/subsystems/rotorcraft/actuators_mkk.makefile
+++ b/conf/autopilot/subsystems/rotorcraft/actuators_mkk.makefile
@@ -38,12 +38,10 @@ endif
ap.srcs += $(SRC_FIRMWARE)/actuators/supervision.c
ap.srcs += $(SRC_FIRMWARE)/actuators/actuators_mkk.c
-ap.srcs += mcu_periph/i2c.c
-ap.srcs += $(SRC_ARCH)/mcu_periph/i2c_arch.c
ifeq ($(ARCH), lpc21)
ap.CFLAGS += -DACTUATORS_MKK_DEVICE=i2c0
-ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=$(MKK_I2C_SCL_TIME) -DI2C0_SCLH=$(MKK_I2C_SCL_TIME) -DI2C0_VIC_SLOT=10
+ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=$(MKK_I2C_SCL_TIME) -DI2C0_SCLH=$(MKK_I2C_SCL_TIME) -DI2C0_VIC_SLOT=11
else ifeq ($(ARCH), stm32)
ap.CFLAGS += -DACTUATORS_MKK_DEVICE=i2c1
ap.CFLAGS += -DUSE_I2C1
diff --git a/conf/autopilot/subsystems/rotorcraft/actuators_pwm_supervision.makefile b/conf/autopilot/subsystems/rotorcraft/actuators_pwm_supervision.makefile
new file mode 100644
index 0000000000..5daf9d055f
--- /dev/null
+++ b/conf/autopilot/subsystems/rotorcraft/actuators_pwm_supervision.makefile
@@ -0,0 +1,7 @@
+
+# add actuatos arch to include directories
+ap.CFLAGS += -I$(SRC_FIRMWARE)/actuators/arch/$(ARCH)
+
+ap.srcs += $(SRC_FIRMWARE)/actuators/supervision.c
+ap.srcs += $(SRC_FIRMWARE)/actuators/actuators_pwm_supervision.c
+ap.srcs += $(SRC_FIRMWARE)/actuators/arch/$(ARCH)/actuators_pwm_arch.c
diff --git a/conf/autopilot/subsystems/rotorcraft/ahrs_cmpl.makefile b/conf/autopilot/subsystems/rotorcraft/ahrs_cmpl.makefile
index 9171256a81..457f08f575 100644
--- a/conf/autopilot/subsystems/rotorcraft/ahrs_cmpl.makefile
+++ b/conf/autopilot/subsystems/rotorcraft/ahrs_cmpl.makefile
@@ -2,12 +2,15 @@
# Fixed point complementary filter using euler angles for attitude estimation
#
-ap.CFLAGS += -DUSE_AHRS_CMPL -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) -DAHRS_FIXED_POINT
+ifdef AHRS_ALIGNER_LED
+ap.CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
+endif
+ap.CFLAGS += -DUSE_AHRS_CMPL
ap.srcs += $(SRC_SUBSYSTEMS)/ahrs.c
ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c
ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_int_cmpl_euler.c
-sim.CFLAGS += -DUSE_AHRS_CMPL -DAHRS_ALIGNER_LED=3 -DAHRS_FIXED_POINT
+sim.CFLAGS += -DUSE_AHRS_CMPL -DAHRS_ALIGNER_LED=3
sim.srcs += $(SRC_SUBSYSTEMS)/ahrs.c
sim.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c
sim.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_int_cmpl_euler.c
diff --git a/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile b/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile
index 1cd8ef6246..592c196ea3 100644
--- a/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile
+++ b/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile
@@ -22,7 +22,7 @@ sim.ARCHDIR = $(ARCH)
sim.CFLAGS += -DSITL -DNPS
sim.CFLAGS += `pkg-config glib-2.0 --cflags` -I /usr/include/meschach
-sim.LDFLAGS += `pkg-config glib-2.0 --libs` -lm -lmeschach -lpcre -lglibivy
+sim.LDFLAGS += `pkg-config glib-2.0 --libs` -lm -lpcre -lglibivy -lgsl -lgslcblas
sim.CFLAGS += -I$(NPSDIR) -I$(SRC_FIRMWARE) -I$(SRC_BOOZ) -I$(SRC_BOOZ_SIM) -I$(SRC_BOARD) -I../simulator -I$(PAPARAZZI_HOME)/conf/simulator/nps
# use the paparazzi-jsbsim package if it is installed, otherwise look for JSBsim under /opt/jsbsim
@@ -65,7 +65,6 @@ sim.srcs += math/pprz_trig_int.c \
sim.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-sim.srcs += $(SRC_BOOZ_SIM)/booz2_unsimulated_peripherals.c
sim.srcs += firmwares/rotorcraft/main.c
sim.srcs += mcu.c
sim.srcs += $(SRC_ARCH)/mcu_arch.c
@@ -75,6 +74,8 @@ sim.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))'
#sim.CFLAGS += -DUSE_LED
sim.srcs += sys_time.c
+sim.srcs += subsystems/settings.c
+sim.srcs += $(SRC_ARCH)/subsystems/settings_arch.c
sim.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport
sim.srcs += $(SRC_FIRMWARE)/telemetry.c \
@@ -90,13 +91,16 @@ sim.srcs += $(SRC_FIRMWARE)/datalink.c
#
-sim.CFLAGS += -DBOOZ2_ANALOG_BARO_LED=2 -DBOOZ2_ANALOG_BARO_PERIOD='SYS_TICS_OF_SEC((1./100.))'
+sim.CFLAGS += -DROTORCRAFT_BARO_LED=2
sim.srcs += $(SRC_BOARD)/baro_board.c
-sim.CFLAGS += -DBOOZ2_ANALOG_BATTERY_PERIOD='SYS_TICS_OF_SEC((1./10.))'
-sim.srcs += $(SRC_FIRMWARE)/battery.c
+sim.CFLAGS += -DUSE_ADC
+sim.srcs += $(SRC_ARCH)/mcu_periph/adc_arch.c
+sim.srcs += subsystems/electrical.c
+# baro has variable offset amplifier on booz board
+#sim.CFLAGS += -DUSE_DAC
+#sim.srcs += $(SRC_ARCH)/mcu_periph/dac_arch.c
-sim.srcs += $(SRC_BOOZ)/booz2_analog.c $(SRC_BOOZ_SIM)/booz2_analog_hw.c
#sim.CFLAGS += -DIMU_TYPE_H=\"imu/imu_b2.h\"
#sim.CFLAGS += -DIMU_B2_VERSION_1_1
diff --git a/conf/autopilot/subsystems/rotorcraft/gps_skytraq.makefile b/conf/autopilot/subsystems/rotorcraft/gps_skytraq.makefile
index e3f8faf488..9a06840e50 100644
--- a/conf/autopilot/subsystems/rotorcraft/gps_skytraq.makefile
+++ b/conf/autopilot/subsystems/rotorcraft/gps_skytraq.makefile
@@ -10,10 +10,5 @@ ifneq ($(GPS_LED),none)
ap.CFLAGS += -DGPS_LED=$(GPS_LED)
endif
-ifeq ($(ARCH), lpc21)
-ap.CFLAGS += -D$(GPS_PORT)_VIC_SLOT=5
-endif
-
-
sim.CFLAGS += -DUSE_GPS
sim.srcs += $(SRC_BOOZ)/booz_gps.c
diff --git a/conf/autopilot/subsystems/rotorcraft/gps_ublox.makefile b/conf/autopilot/subsystems/rotorcraft/gps_ublox.makefile
index 1e476c2c29..784099f03a 100644
--- a/conf/autopilot/subsystems/rotorcraft/gps_ublox.makefile
+++ b/conf/autopilot/subsystems/rotorcraft/gps_ublox.makefile
@@ -10,9 +10,5 @@ ifneq ($(GPS_LED),none)
ap.CFLAGS += -DGPS_LED=$(GPS_LED)
endif
-ifeq ($(ARCH), lpc21)
-ap.CFLAGS += -D$(GPS_PORT)_VIC_SLOT=5
-endif
-
sim.CFLAGS += -DUSE_GPS
sim.srcs += $(SRC_BOOZ)/booz_gps.c
diff --git a/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.0.makefile b/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.0.makefile
index f3074ca2bb..0e435f803d 100644
--- a/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.0.makefile
+++ b/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.0.makefile
@@ -50,7 +50,7 @@ imu_CFLAGS += -DMAX1168_EOC_VIC_SLOT=8
imu_CFLAGS += -DUSE_AMI601
imu_srcs += peripherals/ami601.c
-imu_CFLAGS += -DUSE_I2C1 -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=11
+imu_CFLAGS += -DUSE_I2C1 -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=12
# Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets
diff --git a/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.1.makefile b/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.1.makefile
index e06aeeb7df..afea1237dd 100644
--- a/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.1.makefile
+++ b/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.1.makefile
@@ -42,7 +42,7 @@
# imu Booz2 v1.1
imu_CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_b2.h\"
-imu_CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_MS2001
+imu_CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_MS2100
imu_CFLAGS += -DIMU_B2_VERSION_1_1
imu_srcs += $(SRC_SUBSYSTEMS)/imu.c
imu_srcs += $(SRC_SUBSYSTEMS)/imu/imu_b2.c
@@ -51,13 +51,13 @@ imu_srcs += $(SRC_ARCH)/subsystems/imu/imu_b2_arch.c
imu_srcs += peripherals/max1168.c
imu_srcs += $(SRC_ARCH)/peripherals/max1168_arch.c
-imu_srcs += peripherals/ms2001.c
-imu_srcs += $(SRC_ARCH)/peripherals/ms2001_arch.c
+imu_srcs += peripherals/ms2100.c
+imu_srcs += $(SRC_ARCH)/peripherals/ms2100_arch.c
ifeq ($(ARCH), lpc21)
imu_CFLAGS += -DSSP_VIC_SLOT=9
imu_CFLAGS += -DMAX1168_EOC_VIC_SLOT=8
-imu_CFLAGS += -DMS2001_DRDY_VIC_SLOT=11
+imu_CFLAGS += -DMS2100_DRDY_VIC_SLOT=12
else ifeq ($(ARCH), stm32)
imu_CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ
imu_CFLAGS += -DMAX_1168_DRDY_PORT=$(MAX_1168_DRDY_PORT)
diff --git a/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.2.makefile b/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.2.makefile
index e8f0930cc0..91ba3d16d4 100644
--- a/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.2.makefile
+++ b/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.2.makefile
@@ -57,8 +57,8 @@ imu_srcs += $(SRC_ARCH)/peripherals/hmc5843_arch.c
ifeq ($(ARCH), lpc21)
imu_CFLAGS += -DSSP_VIC_SLOT=9
imu_CFLAGS += -DMAX1168_EOC_VIC_SLOT=8
-#FIXME ms2001 not used on this imu
-imu_CFLAGS += -DMS2001_DRDY_VIC_SLOT=11
+#FIXME ms2100 not used on this imu
+imu_CFLAGS += -DMS2100_DRDY_VIC_SLOT=12
else ifeq ($(ARCH), stm32)
imu_CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ
imu_CFLAGS += -DMAX_1168_DRDY_PORT=$(MAX_1168_DRDY_PORT)
diff --git a/conf/autopilot/subsystems/rotorcraft/imu_crista.makefile b/conf/autopilot/subsystems/rotorcraft/imu_crista.makefile
index 449da2f909..aa1dfb9cc9 100644
--- a/conf/autopilot/subsystems/rotorcraft/imu_crista.makefile
+++ b/conf/autopilot/subsystems/rotorcraft/imu_crista.makefile
@@ -70,7 +70,7 @@ imu_srcs += peripherals/ami601.c
imu_CFLAGS += -DUSE_I2C1
ifeq ($(ARCH), lpc21)
-imu_CFLAGS += -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=11 -DI2C1_BUF_LEN=16
+imu_CFLAGS += -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=12 -DI2C1_BUF_LEN=16
else ifeq ($(ARCH), stm32)
#FIXME
endif
diff --git a/conf/autopilot/subsystems/rotorcraft/telemetry_transparent.makefile b/conf/autopilot/subsystems/rotorcraft/telemetry_transparent.makefile
index fbc1b89c2e..920b031d66 100644
--- a/conf/autopilot/subsystems/rotorcraft/telemetry_transparent.makefile
+++ b/conf/autopilot/subsystems/rotorcraft/telemetry_transparent.makefile
@@ -12,7 +12,3 @@ ap.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=$(MODEM_PORT) -DPPRZ_UART=$(MODEM_PORT
ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ
ap.srcs += downlink.c pprz_transport.c
ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/telemetry.c
-
-ifeq ($(ARCH), lpc21)
-ap.CFLAGS += -D$(MODEM_PORT)_VIC_SLOT=6
-endif
diff --git a/conf/autopilot/subsystems/rotorcraft/telemetry_transparent_usb.makefile b/conf/autopilot/subsystems/rotorcraft/telemetry_transparent_usb.makefile
new file mode 100644
index 0000000000..a15920babb
--- /dev/null
+++ b/conf/autopilot/subsystems/rotorcraft/telemetry_transparent_usb.makefile
@@ -0,0 +1,16 @@
+
+#serial USB (e.g. /dev/ttyACM0)
+
+ifeq ($(ARCH), lpc21)
+ap.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=UsbS -DPPRZ_UART=UsbS
+ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ -DUSE_USB_SERIAL
+ap.srcs += downlink.c pprz_transport.c
+ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/telemetry.c
+ap.srcs += $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbcontrol.c
+ap.srcs += $(SRC_ARCH)/lpcusb/usbstdreq.c $(SRC_ARCH)/lpcusb/usbinit.c
+else
+ifneq ($(ARCH), sim)
+$(error telemetry_transparent_usb currently only implemented for the lpc21)
+endif
+endif
+
diff --git a/conf/autopilot/subsystems/rotorcraft/telemetry_xbee_api.makefile b/conf/autopilot/subsystems/rotorcraft/telemetry_xbee_api.makefile
index a10f3fa95e..d083461928 100644
--- a/conf/autopilot/subsystems/rotorcraft/telemetry_xbee_api.makefile
+++ b/conf/autopilot/subsystems/rotorcraft/telemetry_xbee_api.makefile
@@ -13,7 +13,3 @@ ap.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=$(MODEM_PORT) -DXBEE_UART=$(MODEM_PORT
ap.CFLAGS += -DDOWNLINK_TRANSPORT=XBeeTransport -DDATALINK=XBEE
ap.srcs += downlink.c xbee.c
ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/telemetry.c
-
-ifeq ($(ARCH), lpc21)
-ap.CFLAGS += -D$(MODEM_PORT)_VIC_SLOT=6
-endif
diff --git a/conf/autopilot/subsystems/shared/ahrs_ic.makefile b/conf/autopilot/subsystems/shared/ahrs_ic.makefile
new file mode 100644
index 0000000000..bda8b08365
--- /dev/null
+++ b/conf/autopilot/subsystems/shared/ahrs_ic.makefile
@@ -0,0 +1,22 @@
+#
+# AHRS_PROPAGATE_FREQUENCY
+# AHRS_H_X
+# AHRS_H_Y
+# AHRS_H_Z
+#
+
+AHRS_CFLAGS = -DUSE_AHRS
+ifdef AHRS_ALIGNER_LED
+AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
+endif
+AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl.h\"
+AHRS_SRCS += subsystems/ahrs.c
+AHRS_SRCS += subsystems/ahrs/ahrs_int_cmpl.c
+AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c
+
+ap.CFLAGS += $(AHRS_CFLAGS)
+ap.srcs += $(AHRS_SRCS)
+
+sim.CFLAGS += $(AHRS_CFLAGS)
+sim.srcs += $(AHRS_SRCS)
+
diff --git a/conf/autopilot/subsystems/shared/imu_aspirin.makefile b/conf/autopilot/subsystems/shared/imu_aspirin.makefile
new file mode 100644
index 0000000000..29d685d4c9
--- /dev/null
+++ b/conf/autopilot/subsystems/shared/imu_aspirin.makefile
@@ -0,0 +1,17 @@
+
+IMU_ASPIRIN_CFLAGS = -DUSE_IMU
+IMU_ASPIRIN_CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin.h\" -DIMU_OVERRIDE_CHANNELS
+IMU_ASPIRIN_SRCS = $(SRC_SUBSYSTEMS)/imu.c \
+ $(SRC_SUBSYSTEMS)/imu/imu_aspirin.c \
+ $(SRC_ARCH)/subsystems/imu/imu_aspirin_arch.c \
+ math/pprz_trig_int.c
+IMU_ASPIRIN_SRCS += peripherals/hmc5843.c $(SRC_ARCH)/peripherals/hmc5843_arch.c
+IMU_ASPIRIN_CFLAGS += -DUSE_I2C2
+IMU_ASPIRIN_SRCS += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c
+IMU_ASPIRIN_CFLAGS += -DUSE_EXTI15_10_IRQ # Gyro Int on PC14
+IMU_ASPIRIN_CFLAGS += -DUSE_EXTI9_5_IRQ # Mag Int on PB5
+IMU_ASPIRIN_CFLAGS += -DUSE_EXTI2_IRQ # Accel Int on PD2
+IMU_ASPIRIN_CFLAGS += -DUSE_DMA1_C4_IRQ # SPI2 Rx DMA
+
+ap.CFLAGS += $(IMU_ASPIRIN_CFLAGS)
+ap.srcs += $(IMU_ASPIRIN_SRCS)
diff --git a/conf/autopilot/subsystems/shared/radio_control_ppm.makefile b/conf/autopilot/subsystems/shared/radio_control_ppm.makefile
index e5479f0e9e..6b2fdeab88 100644
--- a/conf/autopilot/subsystems/shared/radio_control_ppm.makefile
+++ b/conf/autopilot/subsystems/shared/radio_control_ppm.makefile
@@ -19,9 +19,8 @@ ifeq ($(NORADIO), False)
$(TARGET).CFLAGS += -DRADIO_CONTROL_TYPE_PPM
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/radio_control.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/radio_control/ppm.c
- ifneq ($(ARCH),jsbsim)
- $(TARGET).srcs += $(SRC_ARCH)/subsystems/radio_control/ppm_arch.c
- endif
+ $(TARGET).srcs += $(SRC_ARCH)/subsystems/radio_control/ppm_arch.c
+
ifeq ($(ARCH),stm32)
ap.CFLAGS += -DUSE_TIM2_IRQ
endif
diff --git a/conf/boards/hb_1.1.makefile b/conf/boards/hb_1.1.makefile
index 88a5accd29..b8d0c3ae0a 100644
--- a/conf/boards/hb_1.1.makefile
+++ b/conf/boards/hb_1.1.makefile
@@ -36,6 +36,7 @@ ifndef GPS_BAUD
GPS_BAUD = B38400
endif
+GPS_LED = 2
ifndef ADC_IR1
ADC_IR1 = 1
@@ -53,3 +54,4 @@ ifndef ADC_IR_NB_SAMPLES
ADC_IR_NB_SAMPLES = 16
endif
+$(TARGET).ARCHDIR = $(ARCH)
diff --git a/conf/boards/lisa_m_1.0.makefile b/conf/boards/lisa_m_1.0.makefile
index 78de8610ed..0a05903f17 100644
--- a/conf/boards/lisa_m_1.0.makefile
+++ b/conf/boards/lisa_m_1.0.makefile
@@ -27,7 +27,7 @@ endif
#
#
-SYS_TIME_LED = 2
+SYS_TIME_LED = 1
RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT = UART3
RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT = UART5
diff --git a/conf/control_panel.xml.example b/conf/control_panel.xml.example
index e9a2ea99a1..36f12526fa 100644
--- a/conf/control_panel.xml.example
+++ b/conf/control_panel.xml.example
@@ -63,6 +63,11 @@
+
+
+
+
+
diff --git a/conf/flight_plans/EMAV2008.xml b/conf/flight_plans/EMAV2008.xml
index 61457ec38c..2cadcfc0a2 100644
--- a/conf/flight_plans/EMAV2008.xml
+++ b/conf/flight_plans/EMAV2008.xml
@@ -84,7 +84,7 @@
-
+
@@ -130,7 +130,7 @@
-
+
@@ -149,7 +149,7 @@
-
+
diff --git a/conf/flight_plans/IS.xml b/conf/flight_plans/IS.xml
index 9c264b7f37..dddd2b9cb5 100644
--- a/conf/flight_plans/IS.xml
+++ b/conf/flight_plans/IS.xml
@@ -148,7 +148,7 @@
diff --git a/conf/flight_plans/basic.xml b/conf/flight_plans/basic.xml
index 57f3531d50..f9ead2281c 100644
--- a/conf/flight_plans/basic.xml
+++ b/conf/flight_plans/basic.xml
@@ -78,7 +78,7 @@
-
+
diff --git a/conf/flight_plans/ccc_gl.xml b/conf/flight_plans/ccc_gl.xml
index 47a16f5a85..1a7656c6f6 100644
--- a/conf/flight_plans/ccc_gl.xml
+++ b/conf/flight_plans/ccc_gl.xml
@@ -112,7 +112,7 @@
-
+
diff --git a/conf/flight_plans/fp_tp_auto.xml b/conf/flight_plans/fp_tp_auto.xml
index d9f0b4f940..3f61b05c1b 100644
--- a/conf/flight_plans/fp_tp_auto.xml
+++ b/conf/flight_plans/fp_tp_auto.xml
@@ -111,7 +111,7 @@
-
+
diff --git a/conf/flight_plans/grosslobke_demo.xml b/conf/flight_plans/grosslobke_demo.xml
index 03e7dee1a1..92cecfe5f7 100755
--- a/conf/flight_plans/grosslobke_demo.xml
+++ b/conf/flight_plans/grosslobke_demo.xml
@@ -77,7 +77,7 @@
-
+
diff --git a/conf/flight_plans/joystick.xml b/conf/flight_plans/joystick.xml
index 55b6b6e0cd..585a5542e4 100644
--- a/conf/flight_plans/joystick.xml
+++ b/conf/flight_plans/joystick.xml
@@ -80,7 +80,7 @@
-
+
diff --git a/conf/flight_plans/landing.xml b/conf/flight_plans/landing.xml
index bcbc1cf8b0..953b26464b 100644
--- a/conf/flight_plans/landing.xml
+++ b/conf/flight_plans/landing.xml
@@ -18,7 +18,7 @@
-
+
diff --git a/conf/flight_plans/mav07.xml b/conf/flight_plans/mav07.xml
index fb2a87092a..d4e3429ae0 100644
--- a/conf/flight_plans/mav07.xml
+++ b/conf/flight_plans/mav07.xml
@@ -130,7 +130,7 @@
-
+
diff --git a/conf/flight_plans/mav08.xml b/conf/flight_plans/mav08.xml
index d418ee1cf7..084723612c 100644
--- a/conf/flight_plans/mav08.xml
+++ b/conf/flight_plans/mav08.xml
@@ -98,7 +98,7 @@
-
+
diff --git a/conf/flight_plans/nordlys.xml b/conf/flight_plans/nordlys.xml
index 61fa5a0fa6..9e279fe850 100644
--- a/conf/flight_plans/nordlys.xml
+++ b/conf/flight_plans/nordlys.xml
@@ -85,7 +85,7 @@
-
+
diff --git a/conf/flight_plans/poles.xml b/conf/flight_plans/poles.xml
index fad085ec5e..e1476ff1d8 100644
--- a/conf/flight_plans/poles.xml
+++ b/conf/flight_plans/poles.xml
@@ -70,7 +70,7 @@
-
+
diff --git a/conf/flight_plans/basic_booz.xml b/conf/flight_plans/rotorcraft_basic.xml
similarity index 100%
rename from conf/flight_plans/basic_booz.xml
rename to conf/flight_plans/rotorcraft_basic.xml
diff --git a/conf/flight_plans/slayer_training.xml b/conf/flight_plans/slayer_training.xml
index 16773c0183..92d84fe58c 100644
--- a/conf/flight_plans/slayer_training.xml
+++ b/conf/flight_plans/slayer_training.xml
@@ -75,7 +75,7 @@
-
+
diff --git a/conf/flight_plans/snav.xml b/conf/flight_plans/snav.xml
index 866073c27e..d22bd1c962 100644
--- a/conf/flight_plans/snav.xml
+++ b/conf/flight_plans/snav.xml
@@ -55,7 +55,7 @@
-
+
diff --git a/conf/flight_plans/standard.xml b/conf/flight_plans/standard.xml
index 906defbb70..ffa5e8dbda 100644
--- a/conf/flight_plans/standard.xml
+++ b/conf/flight_plans/standard.xml
@@ -68,7 +68,7 @@
-
+
diff --git a/conf/flight_plans/tcas.xml b/conf/flight_plans/tcas.xml
index c0b327457c..9a3c2ce1ba 100644
--- a/conf/flight_plans/tcas.xml
+++ b/conf/flight_plans/tcas.xml
@@ -72,7 +72,7 @@
-
+
diff --git a/conf/flight_plans/versatile.xml b/conf/flight_plans/versatile.xml
index c5d5e9becc..7544e5dda3 100644
--- a/conf/flight_plans/versatile.xml
+++ b/conf/flight_plans/versatile.xml
@@ -119,7 +119,7 @@
-
+
diff --git a/conf/flight_plans/xsens_cachejunction.xml b/conf/flight_plans/xsens_cachejunction.xml
index 84aa6cf7bf..2179cf4094 100644
--- a/conf/flight_plans/xsens_cachejunction.xml
+++ b/conf/flight_plans/xsens_cachejunction.xml
@@ -89,7 +89,7 @@
-
+
diff --git a/conf/gps/rotorcraft_ublox6.txt b/conf/gps/rotorcraft_ublox6.txt
new file mode 100644
index 0000000000..ed3c8389d5
--- /dev/null
+++ b/conf/gps/rotorcraft_ublox6.txt
@@ -0,0 +1,65 @@
+MON-VER - 0A 04 46 00 37 2E 30 31 20 28 34 34 31 37 39 29 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 30 30 30 34 30 30 30 37 00 00 36 2E 30 32 20 28 33 36 30 32 33 29 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
+CFG-ANT - 06 13 04 00 1B 00 8B A9
+CFG-DAT - 06 06 02 00 00 00
+CFG-FXN - 06 0E 24 00 0C 00 00 00 00 00 00 00 00 00 00 00 10 27 00 00 10 27 00 00 D0 07 00 00 18 FC FF FF 00 00 00 00 00 00 00 00
+CFG-INF - 06 02 0A 00 00 00 00 00 00 00 00 00 00 00
+CFG-INF - 06 02 0A 00 01 00 00 00 87 87 87 87 87 87
+CFG-INF - 06 02 0A 00 03 00 00 00 00 00 00 00 00 00
+CFG-MSG - 06 01 08 00 01 01 00 00 00 00 00 00
+CFG-MSG - 06 01 08 00 01 02 00 05 00 00 00 00
+CFG-MSG - 06 01 08 00 01 03 00 00 00 00 00 00
+CFG-MSG - 06 01 08 00 01 04 00 00 00 00 00 00
+CFG-MSG - 06 01 08 00 01 06 00 01 00 00 00 00
+CFG-MSG - 06 01 08 00 01 11 00 00 00 00 00 00
+CFG-MSG - 06 01 08 00 01 12 00 00 00 00 00 00
+CFG-MSG - 06 01 08 00 01 20 00 00 00 00 00 00
+CFG-MSG - 06 01 08 00 01 21 00 00 00 00 00 00
+CFG-MSG - 06 01 08 00 01 22 00 00 00 00 00 00
+CFG-MSG - 06 01 08 00 01 30 00 04 00 00 00 00
+CFG-MSG - 06 01 08 00 01 31 00 00 00 00 00 00
+CFG-MSG - 06 01 08 00 01 32 00 00 00 00 00 00
+CFG-MSG - 06 01 08 00 02 20 00 00 00 00 00 00
+CFG-MSG - 06 01 08 00 02 23 00 00 00 00 00 00
+CFG-MSG - 06 01 08 00 0A 02 00 00 00 00 00 00
+CFG-MSG - 06 01 08 00 0A 05 00 00 00 00 00 00
+CFG-MSG - 06 01 08 00 0A 06 00 00 00 00 00 00
+CFG-MSG - 06 01 08 00 0A 07 00 00 00 00 00 00
+CFG-MSG - 06 01 08 00 0A 08 00 00 00 00 00 00
+CFG-MSG - 06 01 08 00 0A 09 00 00 00 00 00 00
+CFG-MSG - 06 01 08 00 0A 0A 00 00 00 00 00 00
+CFG-MSG - 06 01 08 00 0A 20 00 00 00 00 00 00
+CFG-MSG - 06 01 08 00 0A 21 00 00 00 00 00 00
+CFG-MSG - 06 01 08 00 0B 00 00 00 00 00 00 00
+CFG-MSG - 06 01 08 00 0B 30 00 00 00 00 00 00
+CFG-MSG - 06 01 08 00 0B 31 00 00 00 00 00 00
+CFG-MSG - 06 01 08 00 0B 32 00 00 00 00 00 00
+CFG-MSG - 06 01 08 00 0D 01 00 00 00 00 00 00
+CFG-MSG - 06 01 08 00 0D 03 00 00 00 00 00 00
+CFG-MSG - 06 01 08 00 F0 00 01 01 01 01 01 01
+CFG-MSG - 06 01 08 00 F0 01 01 01 01 01 01 01
+CFG-MSG - 06 01 08 00 F0 02 01 01 01 01 01 01
+CFG-MSG - 06 01 08 00 F0 03 01 01 01 01 01 01
+CFG-MSG - 06 01 08 00 F0 04 01 01 01 01 01 01
+CFG-MSG - 06 01 08 00 F0 05 01 01 01 01 01 01
+CFG-MSG - 06 01 08 00 F0 06 00 00 00 00 00 00
+CFG-MSG - 06 01 08 00 F0 07 00 00 00 00 00 00
+CFG-MSG - 06 01 08 00 F0 08 00 00 00 00 00 00
+CFG-MSG - 06 01 08 00 F0 09 00 00 00 00 00 00
+CFG-MSG - 06 01 08 00 F0 0A 00 00 00 00 00 00
+CFG-MSG - 06 01 08 00 F1 00 00 00 00 00 00 00
+CFG-MSG - 06 01 08 00 F1 03 00 00 00 00 00 00
+CFG-MSG - 06 01 08 00 F1 04 00 00 00 00 00 00
+CFG-NAV5 - 06 24 24 00 FF FF 08 02 00 00 00 00 10 27 00 00 05 00 FA 00 FA 00 64 00 2C 01 00 3C 00 00 00 00 00 00 00 00 00 00 00 00
+CFG-NAVX5 - 06 23 28 00 00 00 FF FF 03 00 00 00 03 02 03 10 07 00 00 01 00 00 43 06 00 00 00 00 01 01 00 00 00 64 78 00 00 00 00 00 00 00 00 00
+CFG-NMEA - 06 17 04 00 00 23 00 02
+CFG-PM - 06 32 18 00 00 06 00 00 04 90 00 00 E8 03 00 00 10 27 00 00 00 00 00 00 02 00 00 00
+CFG-PRT - 06 00 14 00 00 00 00 00 84 00 00 00 00 00 00 00 07 00 07 00 00 00 00 00
+CFG-PRT - 06 00 14 00 01 00 00 00 C0 08 00 00 00 E1 00 00 07 00 01 00 00 00 00 00
+CFG-PRT - 06 00 14 00 02 00 00 00 C0 08 00 00 80 25 00 00 00 00 00 00 00 00 00 00
+CFG-PRT - 06 00 14 00 03 00 00 00 00 00 00 00 00 00 00 00 07 00 07 00 00 00 00 00
+CFG-RATE - 06 08 06 00 7D 00 01 00 01 00
+CFG-RXM - 06 11 02 00 08 00
+CFG-SBAS - 06 16 08 00 03 03 03 00 51 08 00 00
+CFG-TP - 06 07 14 00 40 42 0F 00 A0 86 01 00 01 01 00 00 32 00 00 00 00 00 00 00
+CFG-TP5 - 06 31 20 00 00 E9 03 00 32 00 00 00 40 42 0F 00 40 42 0F 00 00 00 00 00 A0 86 01 00 00 00 00 00 F7 00 00 00
+CFG-USB - 06 1B 6C 00 46 15 A6 01 00 00 00 00 64 00 00 01 75 2D 62 6C 6F 78 20 41 47 20 2D 20 77 77 77 2E 75 2D 62 6C 6F 78 2E 63 6F 6D 00 00 00 00 00 00 75 2D 62 6C 6F 78 20 36 20 20 2D 20 20 47 50 53 20 52 65 63 65 69 76 65 72 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
diff --git a/sw/ground_segment/joystick/attack3_booz_nav.xml b/conf/joystick/attack3_booz_nav.xml
similarity index 73%
rename from sw/ground_segment/joystick/attack3_booz_nav.xml
rename to conf/joystick/attack3_booz_nav.xml
index 583f317102..fbeb7114ee 100644
--- a/sw/ground_segment/joystick/attack3_booz_nav.xml
+++ b/conf/joystick/attack3_booz_nav.xml
@@ -11,8 +11,17 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
+
diff --git a/conf/joystick/aurora_skate_controller.xml b/conf/joystick/aurora_skate_controller.xml
new file mode 100644
index 0000000000..e803f2707f
--- /dev/null
+++ b/conf/joystick/aurora_skate_controller.xml
@@ -0,0 +1,144 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/joystick/cockpit_sx.xml b/conf/joystick/cockpit_sx.xml
new file mode 100644
index 0000000000..8bfa250d4d
--- /dev/null
+++ b/conf/joystick/cockpit_sx.xml
@@ -0,0 +1,22 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/joystick/ikarus_usb.xml b/conf/joystick/ikarus_usb.xml
new file mode 100644
index 0000000000..1f5bb2a379
--- /dev/null
+++ b/conf/joystick/ikarus_usb.xml
@@ -0,0 +1,20 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/joystick/logitech_dual_action.xml b/conf/joystick/logitech_dual_action.xml
new file mode 100644
index 0000000000..e9d7572d7c
--- /dev/null
+++ b/conf/joystick/logitech_dual_action.xml
@@ -0,0 +1,87 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/joystick/logitech_f710.xml b/conf/joystick/logitech_f710.xml
new file mode 100644
index 0000000000..dc6c75ef6b
--- /dev/null
+++ b/conf/joystick/logitech_f710.xml
@@ -0,0 +1,153 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/ground_segment/joystick/ngs_set_actuators.xml b/conf/joystick/ngs_set_actuators.xml
similarity index 100%
rename from sw/ground_segment/joystick/ngs_set_actuators.xml
rename to conf/joystick/ngs_set_actuators.xml
diff --git a/sw/ground_segment/joystick/xbox_booz_nav.xml b/conf/joystick/xbox_booz_nav.xml
similarity index 100%
rename from sw/ground_segment/joystick/xbox_booz_nav.xml
rename to conf/joystick/xbox_booz_nav.xml
diff --git a/conf/messages.xml b/conf/messages.xml
index 2029c96743..4c58df1c40 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -439,7 +439,18 @@
-
+
+
+
+
+
+
+
+
+
+
+
+
@@ -823,19 +834,19 @@
-
+
-
+
-
+
@@ -1316,7 +1327,13 @@
-
+
+
+
+
+
+
+
@@ -1586,7 +1603,12 @@
-
+
+
+
+
+
+
@@ -1932,6 +1954,15 @@
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/airborne_ant_track.xml b/conf/modules/airborne_ant_track.xml
new file mode 100644
index 0000000000..c8cc079709
--- /dev/null
+++ b/conf/modules/airborne_ant_track.xml
@@ -0,0 +1,13 @@
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/ins_arduimu.xml b/conf/modules/ins_arduimu.xml
index f3e4074b1d..37d4fee9bc 100644
--- a/conf/modules/ins_arduimu.xml
+++ b/conf/modules/ins_arduimu.xml
@@ -10,8 +10,8 @@
-
+
+
+
diff --git a/conf/modules/ins_arduimu_basic.xml b/conf/modules/ins_arduimu_basic.xml
new file mode 100644
index 0000000000..bc29ed822e
--- /dev/null
+++ b/conf/modules/ins_arduimu_basic.xml
@@ -0,0 +1,18 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/servo_switch.xml b/conf/modules/servo_switch.xml
index f6d93e2a34..9255184d55 100644
--- a/conf/modules/servo_switch.xml
+++ b/conf/modules/servo_switch.xml
@@ -6,7 +6,7 @@
-
+
+
-
-
-
-
-
+
+
+
+
+
+
+
+
diff --git a/conf/radios/T9cap.xml b/conf/radios/T9cap.xml
old mode 100755
new mode 100644
diff --git a/conf/radios/vanguard.xml b/conf/radios/vanguard.xml
old mode 100755
new mode 100644
diff --git a/conf/settings/flight_params.xml b/conf/settings/flight_params.xml
new file mode 100644
index 0000000000..1fcf980283
--- /dev/null
+++ b/conf/settings/flight_params.xml
@@ -0,0 +1,34 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/settings/fw_ctl_n.xml b/conf/settings/fw_ctl_n.xml
index eb3a8ac9e7..df393b0711 100644
--- a/conf/settings/fw_ctl_n.xml
+++ b/conf/settings/fw_ctl_n.xml
@@ -4,13 +4,19 @@
-
+
+
+
+
+
+
+
@@ -54,8 +60,6 @@
-
-
diff --git a/conf/settings/infrared.xml b/conf/settings/infrared.xml
new file mode 100644
index 0000000000..ee2d434796
--- /dev/null
+++ b/conf/settings/infrared.xml
@@ -0,0 +1,20 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/settings/ins_basic.xml b/conf/settings/ins_basic.xml
new file mode 100644
index 0000000000..386a15c7db
--- /dev/null
+++ b/conf/settings/ins_basic.xml
@@ -0,0 +1,10 @@
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/settings/ir_i2c.xml b/conf/settings/ir_i2c.xml
index 40304d0c2a..b386b4c6d9 100644
--- a/conf/settings/ir_i2c.xml
+++ b/conf/settings/ir_i2c.xml
@@ -3,6 +3,17 @@
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/settings/pbn.xml b/conf/settings/pbn.xml
new file mode 100644
index 0000000000..97f53f5f32
--- /dev/null
+++ b/conf/settings/pbn.xml
@@ -0,0 +1,9 @@
+
+
+
+
+
+
+
+
+
diff --git a/conf/settings/settings.dtd b/conf/settings/settings.dtd
index 25cda6c3be..bde71f467e 100644
--- a/conf/settings/settings.dtd
+++ b/conf/settings/settings.dtd
@@ -36,6 +36,7 @@ unit CDATA #IMPLIED
alt_unit CDATA #IMPLIED
alt_unit_coef CDATA #IMPLIED
values CDATA #IMPLIED
+persistent CDATA #IMPLIED
>
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/settings/tuning_ctl_new.xml b/conf/settings/tuning_ctl_new.xml
index 06caf025ac..8ace2c0b9e 100644
--- a/conf/settings/tuning_ctl_new.xml
+++ b/conf/settings/tuning_ctl_new.xml
@@ -4,67 +4,27 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
-
+
-
-
-
+
+
+
+
-
-
-
-
+
+
+
+
@@ -77,11 +37,11 @@
-
+
-
+
@@ -114,13 +74,10 @@
-
-
-
diff --git a/conf/settings/tuning_pers.xml b/conf/settings/tuning_pers.xml
new file mode 100644
index 0000000000..28ee45a89f
--- /dev/null
+++ b/conf/settings/tuning_pers.xml
@@ -0,0 +1,109 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/simulator/Malolo1/Malolo1-IC.xml b/conf/simulator/Malolo1/Malolo1-IC.xml
new file mode 100644
index 0000000000..89c46bef55
--- /dev/null
+++ b/conf/simulator/Malolo1/Malolo1-IC.xml
@@ -0,0 +1,49 @@
+
+
+
+
+
+
+ 43.46223
+ 1.27289
+ 186.0
+ 2.0
+
+ 0.0
+ 0.0
+ 0.0
+
+ 0.0
+ 20.0
+
+ 0.0
+ -1
+
diff --git a/conf/telemetry/fw_h_ctl_a.xml b/conf/telemetry/fw_h_ctl_a.xml
index f8b3dc8d19..7ec85992a5 100644
--- a/conf/telemetry/fw_h_ctl_a.xml
+++ b/conf/telemetry/fw_h_ctl_a.xml
@@ -38,7 +38,7 @@
-
+
diff --git a/conf/telemetry/telemetry_booz2.xml b/conf/telemetry/telemetry_booz2.xml
index ee9f58aac8..4852249e86 100644
--- a/conf/telemetry/telemetry_booz2.xml
+++ b/conf/telemetry/telemetry_booz2.xml
@@ -40,9 +40,9 @@
-
-
-
+
+
+
diff --git a/conf/telemetry/telemetry_booz2_flixr.xml b/conf/telemetry/telemetry_booz2_flixr.xml
deleted file mode 100644
index 441e1d228f..0000000000
--- a/conf/telemetry/telemetry_booz2_flixr.xml
+++ /dev/null
@@ -1,124 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/data/pictures/gcs_icons/bungee_launch.png b/data/pictures/gcs_icons/bungee_launch.png
new file mode 100644
index 0000000000..246f9832ed
Binary files /dev/null and b/data/pictures/gcs_icons/bungee_launch.png differ
diff --git a/data/pictures/gcs_icons/cam_lock.png b/data/pictures/gcs_icons/cam_lock.png
new file mode 100644
index 0000000000..2bd61ff8bc
Binary files /dev/null and b/data/pictures/gcs_icons/cam_lock.png differ
diff --git a/data/pictures/gcs_icons/cam_unlock.png b/data/pictures/gcs_icons/cam_unlock.png
new file mode 100644
index 0000000000..0079b59308
Binary files /dev/null and b/data/pictures/gcs_icons/cam_unlock.png differ
diff --git a/data/pictures/gcs_icons/decide_landing_dir.png b/data/pictures/gcs_icons/decide_landing_dir.png
new file mode 100644
index 0000000000..12df84bb18
Binary files /dev/null and b/data/pictures/gcs_icons/decide_landing_dir.png differ
diff --git a/data/pictures/gcs_icons/observe.png b/data/pictures/gcs_icons/observe.png
new file mode 100644
index 0000000000..dd6d6f9e94
Binary files /dev/null and b/data/pictures/gcs_icons/observe.png differ
diff --git a/data/pictures/gcs_icons/parachute.png b/data/pictures/gcs_icons/parachute.png
new file mode 100644
index 0000000000..06b7efba8f
Binary files /dev/null and b/data/pictures/gcs_icons/parachute.png differ
diff --git a/data/pictures/gcs_icons/target.png b/data/pictures/gcs_icons/target.png
new file mode 100644
index 0000000000..afafafe609
Binary files /dev/null and b/data/pictures/gcs_icons/target.png differ
diff --git a/data/srtm/Makefile b/data/srtm/Makefile
new file mode 100644
index 0000000000..0e24687031
--- /dev/null
+++ b/data/srtm/Makefile
@@ -0,0 +1,31 @@
+DATADIR = $(PAPARAZZI_HOME)/conf/srtm_data
+Q=@
+
+SRTMData: $(DATADIR)/Africa $(DATADIR)/Australia $(DATADIR)/Eurasia $(DATADIR)/Islands $(DATADIR)/North_America $(DATADIR)/South_America
+
+$(DATADIR):
+ mkdir $(DATADIR)
+
+$(DATADIR)/Africa: $(DATADIR)
+ wget -O $(@) http://dds.cr.usgs.gov/srtm/version2_1/SRTM3/$(@F)
+
+$(DATADIR)/Australia: $(DATADIR)
+ wget -O $(@) http://dds.cr.usgs.gov/srtm/version2_1/SRTM3/$(@F)
+
+$(DATADIR)/Eurasia: $(DATADIR)
+ wget -O $(@) http://dds.cr.usgs.gov/srtm/version2_1/SRTM3/$(@F)
+
+$(DATADIR)/Islands: $(DATADIR)
+ wget -O $(@) http://dds.cr.usgs.gov/srtm/version2_1/SRTM3/$(@F)
+
+$(DATADIR)/North_America: $(DATADIR)
+ wget -O $(@) http://dds.cr.usgs.gov/srtm/version2_1/SRTM3/$(@F)
+
+$(DATADIR)/South_America: $(DATADIR)
+ wget -O $(@) http://dds.cr.usgs.gov/srtm/version2_1/SRTM3/$(@F)
+
+.SUFFIXES: .hgt.zip
+
+%.hgt.zip: SRTMData
+ $(Q)wget -c -nv -N http://dds.cr.usgs.gov/srtm/version2_1/SRTM3/$(shell grep -l $(@F) $(DATADIR)/* | sed -e s#$(DATADIR)/##)/$(@F)
+
diff --git a/doc/pprz_algebra/quaternion.tex b/doc/pprz_algebra/quaternion.tex
index 7977222283..3cb937575f 100644
--- a/doc/pprz_algebra/quaternion.tex
+++ b/doc/pprz_algebra/quaternion.tex
@@ -162,8 +162,8 @@ It is also possible to directly normalise the quaternion
\begin{equation}
\quat{} := \frac{\quat{}}{\norm{\quat{}}}
\end{equation}
-\inHfile{INT32\_QUAT\_NORMALISE(q)}{pprz\_algebra\_int}
-\inHfile{FLOAT\_QUAT\_NORMALISE(q)}{pprz\_algebra\_float}
+\inHfile{INT32\_QUAT\_NORMALIZE(q)}{pprz\_algebra\_int}
+\inHfile{FLOAT\_QUAT\_NORMALIZE(q)}{pprz\_algebra\_float}
\subsection*{Making the real value positive}
It is possible to invert the quaternion if its real value is negative
diff --git a/sw/airborne/ap_downlink.h b/sw/airborne/ap_downlink.h
index c8aa3e389e..041a6a0b66 100644
--- a/sw/airborne/ap_downlink.h
+++ b/sw/airborne/ap_downlink.h
@@ -142,8 +142,8 @@
#include "subsystems/imu.h"
#define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) { DOWNLINK_SEND_IMU_ACCEL_RAW(_chan, &imu.accel_unscaled.x, &imu.accel_unscaled.y, &imu.accel_unscaled.z)}
#define PERIODIC_SEND_IMU_GYRO_RAW(_chan) { DOWNLINK_SEND_IMU_GYRO_RAW(_chan, &imu.gyro_unscaled.p, &imu.gyro_unscaled.q, &imu.gyro_unscaled.r)}
-#define PERIODIC_SEND_IMU_ACCEL(_chan) { DOWNLINK_SEND_IMU_ACCEL_RAW(_chan, &imu.accel.x, &imu.accel.y, &imu.accel.z)}
-#define PERIODIC_SEND_IMU_GYRO(_chan) { DOWNLINK_SEND_IMU_GYRO_RAW(_chan, &imu.gyro.p, &imu.gyro.q, &imu.gyro.r)}
+#define PERIODIC_SEND_IMU_ACCEL(_chan) { struct FloatVect3 accel_float; ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); DOWNLINK_SEND_IMU_ACCEL(_chan, &accel_float.x, &accel_float.y, &accel_float.z)}
+#define PERIODIC_SEND_IMU_GYRO(_chan) { struct FloatRates gyro_float; RATES_FLOAT_OF_BFP(gyro_float, imu.gyro); DOWNLINK_SEND_IMU_GYRO(_chan, &gyro_float.p, &gyro_float.q, &gyro_float.r)}
#else
#define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) {}
#define PERIODIC_SEND_IMU_GYRO_RAW(_chan) {}
diff --git a/sw/airborne/arch/lpc21/LPC2148-ROM-bl.ld b/sw/airborne/arch/lpc21/LPC2148-ROM-bl.ld
index 08391aed14..730c1632e3 100644
--- a/sw/airborne/arch/lpc21/LPC2148-ROM-bl.ld
+++ b/sw/airborne/arch/lpc21/LPC2148-ROM-bl.ld
@@ -7,8 +7,13 @@ STACK_SIZE = 0x1000;
/* Memory Definitions */
MEMORY
{
- ROM (rx) : ORIGIN = 0x00004000, LENGTH = 484k
- RAM (rw) : ORIGIN = 0x40000000, LENGTH = 32k
+ /* 0x00000000: Paparazzi bootloader (16k) */
+ /* 0x00004000: Paparazzi code (480k) */
+ /* 0x0007C000: persistent settings (4k) */
+ /* 0x0007D000: Philips/NXP bootloader (12k) */
+ ROM (rx) : ORIGIN = 0x00004000, LENGTH = 480k
+ /* topmost 32 bytes are reserved for IAP operations */
+ RAM (rw) : ORIGIN = 0x40000000, LENGTH = 32736
}
/* Section Definitions */
@@ -41,6 +46,18 @@ SECTIONS
} > ROM
*****/
+. = ALIGN (4);
+
+.ARM.exidx :
+ {
+ __exidx_start = .;
+ *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+ __exidx_end = .;
+
+ } >ROM
+
+
+
. = ALIGN(4);
/* .ctors .dtors are used for c++ constructors/destructors */
diff --git a/sw/airborne/arch/lpc21/mcu_periph/adc_arch.h b/sw/airborne/arch/lpc21/mcu_periph/adc_arch.h
index 947003f64b..6e5fe748c9 100644
--- a/sw/airborne/arch/lpc21/mcu_periph/adc_arch.h
+++ b/sw/airborne/arch/lpc21/mcu_periph/adc_arch.h
@@ -25,6 +25,8 @@
#ifndef ADC_HW_H
#define ADC_HW_H
+#include BOARD_CONFIG
+
#define AdcBank0(x) (x)
#define AdcBank1(x) (x+NB_ADC)
diff --git a/sw/airborne/arch/lpc21/mcu_periph/dac_arch.c b/sw/airborne/arch/lpc21/mcu_periph/dac_arch.c
new file mode 100644
index 0000000000..adda46e12b
--- /dev/null
+++ b/sw/airborne/arch/lpc21/mcu_periph/dac_arch.c
@@ -0,0 +1,6 @@
+#include "mcu_periph/dac.h"
+
+/* turn on DAC pins */
+void dac_init(void) {
+ PINSEL1 |= 2 << 18;
+}
diff --git a/sw/airborne/arch/lpc21/mcu_periph/dac_arch.h b/sw/airborne/arch/lpc21/mcu_periph/dac_arch.h
new file mode 100644
index 0000000000..e27b929e34
--- /dev/null
+++ b/sw/airborne/arch/lpc21/mcu_periph/dac_arch.h
@@ -0,0 +1,12 @@
+#ifndef LPC21_MCU_PERIPH_DAC_ARCH_H
+#define LPC21_MCU_PERIPH_DAC_ARCH_H
+
+#include "std.h"
+#include "LPC21xx.h"
+
+static inline void DACSet(uint16_t x) {
+ DACR = x << 6;
+}
+
+
+#endif /* LPC21_MCU_PERIPH_DAC_ARCH_H */
diff --git a/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c b/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c
index 378337c50b..f8f02fc83e 100644
--- a/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c
+++ b/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c
@@ -28,7 +28,6 @@
#include "interrupt_hw.h"
#include BOARD_CONFIG
-
///////////////////
// I2C Automaton //
///////////////////
@@ -56,22 +55,18 @@ __attribute__ ((always_inline)) static inline void I2cEndOfTransaction(struct i2
}
}
-__attribute__ ((always_inline)) static inline void I2cFinished(struct i2c_periph* p, struct i2c_transaction* t) {
+__attribute__ ((always_inline)) static inline void I2cSendStop(struct i2c_periph* p, struct i2c_transaction* t) {
+ ((i2cRegs_t *)(p->reg_addr))->conset = _BV(STO);
// transaction finished with success
t->status = I2CTransSuccess;
I2cEndOfTransaction(p);
}
-__attribute__ ((always_inline)) static inline void I2cSendStop(struct i2c_periph* p, struct i2c_transaction* t) {
- ((i2cRegs_t *)(p->reg_addr))->conset = _BV(STO);
- I2cFinished(p,t);
-}
-
__attribute__ ((always_inline)) static inline void I2cFail(struct i2c_periph* p, struct i2c_transaction* t) {
((i2cRegs_t *)(p->reg_addr))->conset = _BV(STO);
+ // transaction failed
t->status = I2CTransFailed;
- p->status = I2CFailed;
- // FIXME I2C should be reseted here
+ // FIXME I2C should be reseted here ?
I2cEndOfTransaction(p);
}
@@ -119,35 +114,7 @@ __attribute__ ((always_inline)) static inline void I2cAutomaton(int32_t state, s
}
else {
/* error , we should have got NACK */
- I2cSendStop(p,trans);
- }
- break;
- case I2C_MR_SLA_ACK: /* At least one char */
- /* Wait and reply with ACK or NACK */
- I2cReceive(p->reg_addr,p->idx_buf < trans->len_r - 1);
- break;
- case I2C_MR_SLA_NACK:
- case I2C_MT_SLA_NACK:
- I2cSendStart(p);
- break;
- case I2C_MT_SLA_ACK:
- case I2C_MT_DATA_ACK:
- if (p->idx_buf < trans->len_w) {
- I2cSendByte(p->reg_addr,trans->buf[p->idx_buf]);
- p->idx_buf++;
- } else {
- if (trans->type == I2CTransTxRx) {
- trans->type = I2CTransRx; /* FIXME should not change type */
- p->idx_buf = 0;
- trans->slave_addr |= 1;
- I2cSendStart(p);
- } else {
- if (trans->stop_after_transmit) {
- I2cSendStop(p,trans);
- } else {
- I2cFinished(p,trans);
- }
- }
+ I2cFail(p,trans);
}
break;
case I2C_MR_DATA_NACK:
@@ -156,9 +123,33 @@ __attribute__ ((always_inline)) static inline void I2cAutomaton(int32_t state, s
}
I2cSendStop(p,trans);
break;
+ case I2C_MR_SLA_ACK: /* At least one char */
+ /* Wait and reply with ACK or NACK */
+ I2cReceive(p->reg_addr,p->idx_buf < trans->len_r - 1);
+ break;
+ case I2C_MR_SLA_NACK:
+ case I2C_MT_SLA_NACK:
+ /* Slave is not responding, transaction is failed */
+ I2cFail(p,trans);
+ break;
+ case I2C_MT_SLA_ACK:
+ case I2C_MT_DATA_ACK:
+ if (p->idx_buf < trans->len_w) {
+ I2cSendByte(p->reg_addr,trans->buf[p->idx_buf]);
+ p->idx_buf++;
+ } else {
+ if (trans->type == I2CTransTxRx) {
+ //trans->type = I2CTransRx; /* FIXME should not change type */
+ p->idx_buf = 0;
+ trans->slave_addr |= 1;
+ I2cSendStart(p);
+ } else {
+ I2cSendStop(p,trans);
+ }
+ }
+ break;
default:
- I2cSendStop(p,trans);
- //I2cFail(p,trans);
+ I2cFail(p,trans);
/* FIXME log error */
break;
}
diff --git a/sw/airborne/arch/lpc21/peripherals/ms2001_arch.c b/sw/airborne/arch/lpc21/peripherals/ms2001_arch.c
deleted file mode 100644
index a75530ac40..0000000000
--- a/sw/airborne/arch/lpc21/peripherals/ms2001_arch.c
+++ /dev/null
@@ -1,50 +0,0 @@
-/* PNI ms2001 connected on SPI1 */
-/*
- IMU b2
- SS on P1.28
- RESET on P1.19
- DRDY on P0.30 ( EINT3)
-*/
-
-#include "peripherals/ms2001.h"
-
-volatile uint8_t ms2001_cur_axe;
-
-static void EXTINT_ISR(void) __attribute__((naked));
-
-void ms2001_arch_init( void ) {
-
- ms2001_cur_axe = 0;
-
- /* configure SS pin */
- Ms2001Unselect(); /* pin idles high */
- SetBit(MS2001_SS_IODIR, MS2001_SS_PIN); /* pin is output */
-
- /* configure RESET pin */
- Ms2001Reset(); /* pin idles low */
- SetBit(MS2001_RESET_IODIR, MS2001_RESET_PIN); /* pin is output */
-
- /* configure DRDY pin */
- /* connected pin to EXINT */
- MS2001_DRDY_PINSEL |= MS2001_DRDY_PINSEL_VAL << MS2001_DRDY_PINSEL_BIT;
- SetBit(EXTMODE, MS2001_DRDY_EINT); /* EINT is edge trigered */
- SetBit(EXTPOLAR,MS2001_DRDY_EINT); /* EINT is trigered on rising edge */
- SetBit(EXTINT,MS2001_DRDY_EINT); /* clear pending EINT */
-
- /* initialize interrupt vector */
- VICIntSelect &= ~VIC_BIT( MS2001_DRDY_VIC_IT ); /* select EINT as IRQ source */
- VICIntEnable = VIC_BIT( MS2001_DRDY_VIC_IT ); /* enable it */
- _VIC_CNTL(MS2001_DRDY_VIC_SLOT) = VIC_ENABLE | MS2001_DRDY_VIC_IT;
- _VIC_ADDR(MS2001_DRDY_VIC_SLOT) = (uint32_t)EXTINT_ISR; // address of the ISR
-
-}
-
-void EXTINT_ISR(void) {
- ISR_ENTRY();
- /* no, we won't do anything asynchronously, so just notify */
- ms2001_status = MS2001_GOT_EOC;
- /* clear EINT */
- EXTINT = (1< // for abs
-
-#include "std.h"
-#include "LPC21xx.h"
-#include "interrupt_hw.h"
-
-#include "ssp_hw.h"
-#include BOARD_CONFIG
-
-#include "generated/airframe.h"
-
-
-
-extern volatile uint8_t ms2001_cur_axe;
-
-#define Ms2001Select() SetBit(MS2001_SS_IOCLR,MS2001_SS_PIN)
-#define Ms2001Unselect() SetBit(MS2001_SS_IOSET,MS2001_SS_PIN)
-
-#define Ms2001Reset() SetBit(MS2001_RESET_IOCLR,MS2001_RESET_PIN)
-#define Ms2001Set() SetBit(MS2001_RESET_IOSET,MS2001_RESET_PIN)
-
-#define Ms2001OnSpiIt() { \
- switch (ms2001_status) { \
- case MS2001_SENDING_REQ: \
- { \
- /* read dummy control byte reply */ \
- uint8_t foo __attribute__ ((unused)) = SSPDR; \
- ms2001_status = MS2001_WAITING_EOC; \
- Ms2001Unselect(); \
- SSP_ClearRti(); \
- SSP_DisableRti(); \
- SSP_Disable(); \
- } \
- break; \
- case MS2001_READING_RES: \
- { \
- int16_t new_val; \
- new_val = SSPDR << 8; \
- new_val += SSPDR; \
- if (abs(new_val) < 2000) \
- ms2001_values[ms2001_cur_axe] = new_val; \
- Ms2001Unselect(); \
- SSP_ClearRti(); \
- SSP_DisableRti(); \
- SSP_Disable(); \
- ms2001_cur_axe++; \
- if (ms2001_cur_axe > 2) { \
- ms2001_cur_axe = 0; \
- ms2001_status = MS2001_DATA_AVAILABLE; \
- } \
- else \
- ms2001_status = MS2001_IDLE; \
- } \
- break; \
- } \
- }
-
-
-#define Ms2001SendReq() { \
- Ms2001Select(); \
- ms2001_status = MS2001_SENDING_REQ; \
- Ms2001Set(); \
- SSP_ClearRti(); \
- SSP_EnableRti(); \
- Ms2001Reset(); \
- uint8_t control_byte = (ms2001_cur_axe+1) << 0 | \
- MS2001_DIVISOR << 4; \
- SSP_Send(control_byte); \
- SSP_Enable(); \
- }
-
-#define Ms2001ReadRes() { \
- ms2001_status = MS2001_READING_RES; \
- Ms2001Select(); \
- /* trigger 2 bytes read */ \
- SSP_Send(0); \
- SSP_Send(0); \
- SSP_Enable(); \
- SSP_ClearRti(); \
- SSP_EnableRti(); \
- }
-
-
-
-#endif /* MS2001_ARCH_H */
diff --git a/sw/airborne/arch/lpc21/peripherals/ms2100_arch.c b/sw/airborne/arch/lpc21/peripherals/ms2100_arch.c
new file mode 100644
index 0000000000..ec22b64b6c
--- /dev/null
+++ b/sw/airborne/arch/lpc21/peripherals/ms2100_arch.c
@@ -0,0 +1,50 @@
+/* PNI ms2100 connected on SPI1 */
+/*
+ IMU b2
+ SS on P1.28
+ RESET on P1.19
+ DRDY on P0.30 ( EINT3)
+*/
+
+#include "peripherals/ms2100.h"
+
+volatile uint8_t ms2100_cur_axe;
+
+static void EXTINT_ISR(void) __attribute__((naked));
+
+void ms2100_arch_init( void ) {
+
+ ms2100_cur_axe = 0;
+
+ /* configure SS pin */
+ Ms2001Unselect(); /* pin idles high */
+ SetBit(MS2100_SS_IODIR, MS2100_SS_PIN); /* pin is output */
+
+ /* configure RESET pin */
+ Ms2001Reset(); /* pin idles low */
+ SetBit(MS2100_RESET_IODIR, MS2100_RESET_PIN); /* pin is output */
+
+ /* configure DRDY pin */
+ /* connected pin to EXINT */
+ MS2100_DRDY_PINSEL |= MS2100_DRDY_PINSEL_VAL << MS2100_DRDY_PINSEL_BIT;
+ SetBit(EXTMODE, MS2100_DRDY_EINT); /* EINT is edge trigered */
+ SetBit(EXTPOLAR,MS2100_DRDY_EINT); /* EINT is trigered on rising edge */
+ SetBit(EXTINT,MS2100_DRDY_EINT); /* clear pending EINT */
+
+ /* initialize interrupt vector */
+ VICIntSelect &= ~VIC_BIT( MS2100_DRDY_VIC_IT ); /* select EINT as IRQ source */
+ VICIntEnable = VIC_BIT( MS2100_DRDY_VIC_IT ); /* enable it */
+ _VIC_CNTL(MS2100_DRDY_VIC_SLOT) = VIC_ENABLE | MS2100_DRDY_VIC_IT;
+ _VIC_ADDR(MS2100_DRDY_VIC_SLOT) = (uint32_t)EXTINT_ISR; // address of the ISR
+
+}
+
+void EXTINT_ISR(void) {
+ ISR_ENTRY();
+ /* no, we won't do anything asynchronously, so just notify */
+ ms2100_status = MS2100_GOT_EOC;
+ /* clear EINT */
+ EXTINT = (1< // for abs
+
+#include "std.h"
+#include "LPC21xx.h"
+#include "interrupt_hw.h"
+
+#include "ssp_hw.h"
+#include BOARD_CONFIG
+
+#include "generated/airframe.h"
+
+
+
+extern volatile uint8_t ms2100_cur_axe;
+
+#define Ms2001Select() SetBit(MS2100_SS_IOCLR,MS2100_SS_PIN)
+#define Ms2001Unselect() SetBit(MS2100_SS_IOSET,MS2100_SS_PIN)
+
+#define Ms2001Reset() SetBit(MS2100_RESET_IOCLR,MS2100_RESET_PIN)
+#define Ms2001Set() SetBit(MS2100_RESET_IOSET,MS2100_RESET_PIN)
+
+#define Ms2001OnSpiInt() { \
+ switch (ms2100_status) { \
+ case MS2100_SENDING_REQ: \
+ { \
+ /* read dummy control byte reply */ \
+ uint8_t foo __attribute__ ((unused)) = SSPDR; \
+ ms2100_status = MS2100_WAITING_EOC; \
+ Ms2001Unselect(); \
+ SSP_ClearRti(); \
+ SSP_DisableRti(); \
+ SSP_Disable(); \
+ } \
+ break; \
+ case MS2100_READING_RES: \
+ { \
+ int16_t new_val; \
+ new_val = SSPDR << 8; \
+ new_val += SSPDR; \
+ if (abs(new_val) < 2000) \
+ ms2100_values[ms2100_cur_axe] = new_val; \
+ Ms2001Unselect(); \
+ SSP_ClearRti(); \
+ SSP_DisableRti(); \
+ SSP_Disable(); \
+ ms2100_cur_axe++; \
+ if (ms2100_cur_axe > 2) { \
+ ms2100_cur_axe = 0; \
+ ms2100_status = MS2100_DATA_AVAILABLE; \
+ } \
+ else \
+ ms2100_status = MS2100_IDLE; \
+ } \
+ break; \
+ } \
+ }
+
+
+#define Ms2001SendReq() { \
+ Ms2001Select(); \
+ ms2100_status = MS2100_SENDING_REQ; \
+ Ms2001Set(); \
+ SSP_ClearRti(); \
+ SSP_EnableRti(); \
+ Ms2001Reset(); \
+ uint8_t control_byte = (ms2100_cur_axe+1) << 0 | \
+ MS2100_DIVISOR << 4; \
+ SSP_Send(control_byte); \
+ SSP_Enable(); \
+ }
+
+#define Ms2001ReadRes() { \
+ ms2100_status = MS2100_READING_RES; \
+ Ms2001Select(); \
+ /* trigger 2 bytes read */ \
+ SSP_Send(0); \
+ SSP_Send(0); \
+ SSP_Enable(); \
+ SSP_ClearRti(); \
+ SSP_EnableRti(); \
+ }
+
+
+
+#endif /* MS2100_ARCH_H */
diff --git a/sw/airborne/arch/lpc21/subsystems/imu/imu_b2_arch.c b/sw/airborne/arch/lpc21/subsystems/imu/imu_b2_arch.c
index 858581dca1..0b98aee11e 100644
--- a/sw/airborne/arch/lpc21/subsystems/imu/imu_b2_arch.c
+++ b/sw/airborne/arch/lpc21/subsystems/imu/imu_b2_arch.c
@@ -26,9 +26,6 @@
int imu_overrun = 0;
volatile uint8_t imu_ssp_status;
static void SSP_ISR(void) __attribute__((naked));
-#if 0
-static inline bool_t isr_try_mag(void);
-#endif
/* SSPCR0 settings */
#define SSP_DDS8 0x07 << 0 /* data size : 8 bits */
@@ -104,91 +101,65 @@ void imu_periodic(void) {
-#include "led.h"
-
-#if 0
-
-
-static inline bool_t isr_try_mag(void) {
- switch (micromag_status) {
- case MS2001_IDLE :
- ImuSetSSP8bits();
- Ms2001SendReq();
- return TRUE;
- case MS2001_GOT_EOC:
- ImuSetSSP8bits();
- Ms2001ReadRes();
- return TRUE;
- }
- return FALSE;
-}
+#if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2100
static void SSP_ISR(void) {
- ISR_ENTRY();
+ ISR_ENTRY();
- switch (imu_ssp_status) {
- case IMU_SSP_STA_BUSY_MAX1168:
- Max1168OnSpiInt();
- if (isr_try_mag())
- imu_ssp_status = IMU_SSP_STA_BUSY_MS2100;
- else
- imu_ssp_status = IMU_SSP_STA_IDLE;
- break;
- case IMU_SSP_STA_BUSY_MS2100:
- Ms2001OnSpiIt();
- imu_ssp_status = IMU_SSP_STA_IDLE;
- break;
- default:
- // spurious interrupt
- LED_ON(1);
- }
+ switch (imu_ssp_status) {
+ case IMU_SSP_STA_BUSY_MAX1168:
+ Max1168OnSpiInt();
+ if (ms2100_status == MS2100_IDLE || ms2100_status == MS2100_GOT_EOC) {
+ ImuSetSSP8bits();
+ if (ms2100_status == MS2100_IDLE) {
+ Ms2001SendReq();
+ }
+ else { /* MS2100_GOT_EOC */
+ Ms2001ReadRes();
+ }
+ imu_ssp_status = IMU_SSP_STA_BUSY_MS2100;
+ }
+ else {
+ imu_ssp_status = IMU_SSP_STA_IDLE;
+ }
+ break;
+ case IMU_SSP_STA_BUSY_MS2100:
+ Ms2001OnSpiInt();
+ if (ms2100_status == MS2100_IDLE) {
+ Ms2001SendReq();
+ imu_ssp_status = IMU_SSP_STA_BUSY_MS2100;
+ }
+ else
+ imu_ssp_status = IMU_SSP_STA_IDLE;
+ break;
- VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */
- ISR_EXIT();
-}
-#endif
-
-
-static void SSP_ISR(void) {
- ISR_ENTRY();
-
- switch (imu_ssp_status) {
- case IMU_SSP_STA_BUSY_MAX1168:
- Max1168OnSpiInt();
-#if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2001
- if (ms2001_status == MS2001_IDLE || ms2001_status == MS2001_GOT_EOC) {
- ImuSetSSP8bits();
- if (ms2001_status == MS2001_IDLE) {
- Ms2001SendReq();
- }
- else { /* MS2001_GOT_EOC */
- Ms2001ReadRes();
- }
- imu_ssp_status = IMU_SSP_STA_BUSY_MS2100;
- }
- else {
-#endif
- imu_ssp_status = IMU_SSP_STA_IDLE;
-#if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2001
- }
-#endif
- break;
-#if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2001
- case IMU_SSP_STA_BUSY_MS2100:
- Ms2001OnSpiIt();
- if (ms2001_status == MS2001_IDLE) {
- Ms2001SendReq();
- imu_ssp_status = IMU_SSP_STA_BUSY_MS2100;
- }
- else
- imu_ssp_status = IMU_SSP_STA_IDLE;
- break;
-#endif
// default:
// spurious interrupt
// FIXME LED_ON(1);
- }
+ }
- VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */
- ISR_EXIT();
+ VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */
+ ISR_EXIT();
}
+
+#else //no IMU_B2_MAG_MS2100
+
+static void SSP_ISR(void) {
+ ISR_ENTRY();
+
+ switch (imu_ssp_status) {
+ case IMU_SSP_STA_BUSY_MAX1168:
+ Max1168OnSpiInt();
+ imu_ssp_status = IMU_SSP_STA_IDLE;
+ break;
+
+ // default:
+ // spurious interrupt
+ // FIXME LED_ON(1);
+ }
+
+ VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */
+ ISR_EXIT();
+}
+
+#endif //no IMU_B2_MAG_MS2100
diff --git a/sw/airborne/arch/lpc21/subsystems/settings_arch.c b/sw/airborne/arch/lpc21/subsystems/settings_arch.c
new file mode 100644
index 0000000000..c493cb6521
--- /dev/null
+++ b/sw/airborne/arch/lpc21/subsystems/settings_arch.c
@@ -0,0 +1,272 @@
+/*
+ * Paparazzi persistent settings low level flash routines lpc21
+ *
+ * Copyright (C) 2011 Martin Mueller
+ *
+ * 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.
+ *
+ */
+
+/*
+ LPC2148 flash data is located in the last available page
+
+ 0x00000000: Paparazzi bootloader (16k)
+ 0x00004000: Paparazzi code (480k)
+ 0x0007C000: persistent settings (4k)
+ 0x0007D000: Philips/NXP bootloader (12k)
+
+ data flash_addr
+ data_size flash_end - FSIZ
+ checksum flash_end - FCHK
+
+ LPC21: minimum write size 256 bytes, endurance 100k cycles,
+ max sector erase time 400ms, max prog time 1ms per 256 bytes
+*/
+
+#include "subsystems/settings.h"
+
+#define IAP_LOCATION 0x7FFFFFF1
+
+#define IAP_PREPARE_SECTORS 50
+#define IAP_COPY_RAM_TO_FLASH 51
+#define IAP_ERASE_SECTORS 52
+#define IAP_BLANK_CHECK_SECTORS 53
+#define IAP_READ_PART_ID 54
+#define IAP_COMPARE 56
+
+/* we have to operate on 256 byte flash boundaries */
+#define BOUND 256
+
+#define FSIZ 8
+#define FCHK 4
+
+typedef void (*IAP)(uint32_t[], uint32_t[]);
+
+typedef struct {
+ uint32_t addr;
+ uint32_t total_size;
+ uint32_t page_nr;
+ uint32_t page_size;
+} FlashInfo;
+
+static uint32_t pflash_checksum(uint32_t ptr, uint32_t size);
+static int32_t flash_detect(FlashInfo* flash);
+static int32_t pflash_erase_page(FlashInfo* flash);
+static int32_t pflash_program_array(FlashInfo* flash,
+ uint32_t dest,
+ uint32_t src);
+static int32_t pflash_program_bytes(FlashInfo* flash,
+ uint32_t src,
+ uint32_t size,
+ uint32_t chksum);
+
+
+static uint32_t pflash_checksum(uint32_t ptr, uint32_t size) {
+ uint32_t i, sum = 0;
+
+ /* do it cheap for now */
+ for (i=0; ipage_size = 0x1000;
+ flash->page_nr = 26;
+ flash->addr = 0x7C000;
+ break;
+ }
+ default: return -1;
+ }
+
+ return 0;
+}
+
+static int32_t pflash_erase_page(FlashInfo* flash) {
+ uint32_t command[5], result[3];
+ IAP iap_entry;
+
+ iap_entry = (IAP) IAP_LOCATION;
+
+ /* prepare page/sector */
+ command[0] = IAP_PREPARE_SECTORS;
+ command[1] = flash->page_nr;
+ command[2] = flash->page_nr;
+ disableIRQ();
+ iap_entry(command, result);
+ enableIRQ();
+ if (result[0] != 0) return result[0];
+
+ /* erase page/sector */
+ command[0] = IAP_ERASE_SECTORS;
+ command[1] = flash->page_nr;
+ command[2] = flash->page_nr;
+ disableIRQ();
+ iap_entry(command, result);
+ enableIRQ();
+ if (result[0] != 0) return result[0];
+
+ /* verify erase */
+ command[0] = IAP_BLANK_CHECK_SECTORS;
+ command[1] = flash->page_nr;
+ command[2] = flash->page_nr;
+ iap_entry(command, result);
+ if (result[0] != 0) return result[0];
+
+ return 0;
+}
+
+static int32_t pflash_program_array(FlashInfo* flash,
+ uint32_t dest,
+ uint32_t src) {
+ uint32_t command[5], result[3];
+ IAP iap_entry;
+
+ iap_entry = (IAP) IAP_LOCATION;
+
+ /* prepare page/sector */
+ command[0] = IAP_PREPARE_SECTORS;
+ command[1] = flash->page_nr;
+ command[2] = flash->page_nr;
+ disableIRQ();
+ iap_entry(command, result);
+ enableIRQ();
+ if (result[0] != 0) return result[0];
+
+ /* flash from ram */
+ command[0] = IAP_COPY_RAM_TO_FLASH;
+ command[1] = dest;
+ command[2] = src;
+ command[3] = BOUND;
+ command[4] = CCLK/1000;
+ disableIRQ();
+ iap_entry(command, result);
+ enableIRQ();
+ if (result[0] != 0) return result[0];
+
+ return 0;
+}
+
+static int32_t pflash_program_bytes(FlashInfo* flash,
+ uint32_t src,
+ uint32_t size,
+ uint32_t chksum) {
+ uint32_t data[BOUND/4], i, j, ret;
+ uint32_t ptr = (uint32_t) &data;
+
+ /* erase */
+ if ((ret = pflash_erase_page(flash))) return ret;
+
+ /* write data in arrays */
+ for (i=0; ipage_size - BOUND) {
+ data[(BOUND-FSIZ)/4] = size;
+ data[(BOUND-FCHK)/4] = chksum;
+ }
+ if ((ret = pflash_program_array(flash, flash->addr+i, ptr))) return ret;
+ }
+
+ /* last array */
+ if (i <= flash->page_size - BOUND) {
+ data[(BOUND-FSIZ)/4] = size;
+ data[(BOUND-FCHK)/4] = chksum;
+ if ((ret = pflash_program_array(flash,
+ flash->addr+flash->page_size-BOUND,
+ ptr)))
+ return ret;
+ }
+
+ /* verify data */
+ for (i=0; iaddr+i)) != (*(uint8_t*) (src+i))) return -2;
+ }
+ if (*(uint32_t*) (flash->addr+flash->page_size-FSIZ) != size) return -3;
+ if (*(uint32_t*) (flash->addr+flash->page_size-FCHK) != chksum) return -4;
+
+ return 0;
+}
+
+int32_t persistent_write(uint32_t ptr, uint32_t size) {
+ FlashInfo flash_info;
+
+ if (flash_detect(&flash_info)) return -1;
+ if ((size > flash_info.page_size-FSIZ) || (size == 0)) return -2;
+
+ return pflash_program_bytes(&flash_info,
+ ptr,
+ size,
+ pflash_checksum(ptr, size));
+}
+
+int32_t persistent_read(uint32_t ptr, uint32_t size) {
+ FlashInfo flash;
+ uint32_t i;
+
+ /* check parameters */
+ if (flash_detect(&flash)) return -1;
+ if ((size > flash.page_size-FSIZ) || (size == 0)) return -2;
+
+ /* check consistency */
+ if (size != *(uint32_t*)(flash.addr+flash.page_size-FSIZ)) return -3;
+ if (pflash_checksum(flash.addr, size) !=
+ *(uint32_t*)(flash.addr+flash.page_size-FCHK))
+ return -4;
+
+ /* copy data */
+ for (i=0; i
+ * Copyright (C) 2011 Martin Mueller
*
- * This file is part of paparazzi.
+ * This file is part of Paparazzi.
*
- * paparazzi is free software; you can redistribute it and/or modify
+ * 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,
+ * 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
+ * along with Paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
+ *
*/
-#ifndef BOOZ2_ANALOG_HW_H
-#define BOOZ2_ANALOG_HW_H
-#define Booz2AnalogSetDAC(x) { }
+#ifndef LPC21_SUBSYSTEMS_SETTINGS_H
+#define LPC21_SUBSYSTEMS_SETTINGS_H
-extern void booz2_analog_init_hw(void);
-#endif /* BOOZ2_ANALOG_HW_H */
+
+#endif /* LPC21_SUBSYSTEMS_SETTINGS_H */
diff --git a/sw/airborne/arch/lpc21/vic_slots.txt b/sw/airborne/arch/lpc21/vic_slots.txt
deleted file mode 100644
index f289a4f48a..0000000000
--- a/sw/airborne/arch/lpc21/vic_slots.txt
+++ /dev/null
@@ -1,15 +0,0 @@
- 1 TIMER0_VIC_SLOT
- 2 AD0_VIC_SLOT
- 4 AD1_VIC_SLOT
- 5 UART0_VIC_SLOT
- 6 UART1_VIC_SLOT
- 9 I2C0_VIC_SLOT
- I2C1_VIC_SLOT
-11 SSP_VIC_SLOT
-11 MS2001_DRDY_VIC_SLOT
-
-12 MICROMAG_DRDY_VIC_SLOT
-12 MAX11040_DRDY_VIC_SLOT
-
-?? MAX1167_EOC_VIC_SLOT
-
diff --git a/sw/airborne/arch/lpc21/vic_slots_fw.txt b/sw/airborne/arch/lpc21/vic_slots_fw.txt
new file mode 100644
index 0000000000..6c94aa7bfe
--- /dev/null
+++ b/sw/airborne/arch/lpc21/vic_slots_fw.txt
@@ -0,0 +1,22 @@
+VIC slots used for fixedwings with the LPC2148
+
+
+define name slot (default) used for
+------------------------------------------------------------------
+TIMER0_VIC_SLOT 1 (1) system timer
+AD0_VIC_SLOT 2 (2) was for adc battery (not needed anymore?)
+hardcoded, no define 3 PWM_ISR in servos_4015
+AD1_VIC_SLOT 4 (4) was for adc baro (not needed anymore?)
+UART0_VIC_SLOT 5 (5) uart_arch, e.g. gps
+UART1_VIC_SLOT 6 (6) uart_arch, e.g. modem
+hardcoded, no define 7 SPI1 in mcu_periph/spi_arch.c, imu_crista_arch, max3100 module, baro_scp module, lcd_dogm module
+MAX1168_EOC_VIC_SLOT 8 imu_b2_v1.0, imu_b2_v1.1, imu_b2_v1.2
+hardcoded, no define 8 EXTINT in max3100 module
+I2C0_VIC_SLOT ? (9) (9 is default in mcu_periph/i2c_arch.c)
+SSP_VIC_SLOT 9 imu_b2_v1.0, imu_b2_v1.1, imu_b2_v1.2
+MICROMAG_DRDY_VIC_SLOT 9 micromag
+hardcoded, no define 10 usb, e.g. telemetry_transparent_usb
+hardcoded, no define 11 EXTINT in baro_scp module
+I2C1_VIC_SLOT 12 ami601 in imu_b2_v1.0, imu_crista
+MS2100_DRDY_VIC_SLOT 12 ms2100 mag in imu_b2_v1.1
+MAX11040_DRDY_VIC_SLOT ? max11040 adc module
diff --git a/sw/airborne/arch/lpc21/vic_slots_rotorcrafts.txt b/sw/airborne/arch/lpc21/vic_slots_rotorcrafts.txt
new file mode 100644
index 0000000000..0acf1429f4
--- /dev/null
+++ b/sw/airborne/arch/lpc21/vic_slots_rotorcrafts.txt
@@ -0,0 +1,18 @@
+VIC slots used for rotorcrafts with the LPC2148
+
+define name slot (default) used for
+------------------------------------------------------------------
+TIMER0_VIC_SLOT 1 (1) system timer
+AD0_VIC_SLOT 2 (2) was for adc battery (not needed anymore?)
+
+AD1_VIC_SLOT 4 (4) was for adc baro (not needed anymore?)
+UART0_VIC_SLOT 5 (5) uart_arch, e.g. gps
+UART1_VIC_SLOT 6 (6) uart_arch, e.g. modem
+hardcoded, no define 7 SPI1 in mcu_periph/spi_arch.c, imu_crista_arch
+MAX1168_EOC_VIC_SLOT 8 imu_b2_v1.0, imu_b2_v1.1, imu_b2_v1.2
+SSP_VIC_SLOT 9 imu_b2_v1.0, imu_b2_v1.1, imu_b2_v1.2
+hardcoded, no define 10 usb, e.g. telemetry_transparent_usb
+I2C0_VIC_SLOT 11 (9) actuators_acstec, actuators_acstec_v2, actuators_mkk, (9 is default in mcu_periph/i2c_arch.c)
+I2C1_VIC_SLOT 12 ami601 in imu_b2_v1.0, imu_crista
+MS2100_DRDY_VIC_SLOT 12 ms2100 mag in imu_b2_v1.1
+
diff --git a/sw/airborne/arch/sim/mcu_periph/i2c_arch.c b/sw/airborne/arch/sim/mcu_periph/i2c_arch.c
index 3aaa5ca976..ceb58123b6 100644
--- a/sw/airborne/arch/sim/mcu_periph/i2c_arch.c
+++ b/sw/airborne/arch/sim/mcu_periph/i2c_arch.c
@@ -2,5 +2,5 @@
void i2c_hw_init ( void ) {}
-bool_t i2c_idle(struct i2c_periph *p) { return TRUE; }
+bool_t i2c_idle(struct i2c_periph *p __attribute__ ((unused))) { return TRUE; }
bool_t i2c_submit(struct i2c_periph* p __attribute__ ((unused)), struct i2c_transaction* t __attribute__ ((unused))) { return TRUE;}
diff --git a/sw/airborne/arch/sim/modules/ins/ins_arduimu.c b/sw/airborne/arch/sim/modules/ins/ins_arduimu.c
new file mode 100644
index 0000000000..05d1f1c9c0
--- /dev/null
+++ b/sw/airborne/arch/sim/modules/ins/ins_arduimu.c
@@ -0,0 +1,49 @@
+/** ArduIMU simulation. OCaml binding.
+ * Sim provides IR sensor reading and airspeed
+ */
+
+
+#include
+#include "generated/airframe.h"
+
+#include
+
+#include "estimator.h"
+
+// Prevent undefined reference (from estimator.c)
+#include "subsystems/sensors/infrared.h"
+struct Infrared infrared;
+
+// Arduimu empty implementation
+#include "modules/ins/ins_arduimu.h"
+
+float ArduIMU_data[NB_DATA];
+
+float ins_roll_neutral;
+float ins_pitch_neutral;
+
+//mixer
+float pitch_of_throttle_gain;
+float throttle_slew;
+
+void ArduIMU_init( void ) {}
+void ArduIMU_periodic( void ) {}
+void ArduIMU_periodicGPS( void ) {}
+void IMU_Daten_verarbeiten( void ) {}
+
+// Updates from Ocaml sim
+float sim_air_speed;
+
+value set_ir_and_airspeed(
+ value roll __attribute__ ((unused)),
+ value front __attribute__ ((unused)),
+ value top __attribute__ ((unused)),
+ value air_speed
+ ) {
+ // Feed directly the estimator
+ estimator_phi = atan2(Int_val(roll), Int_val(top)) - ins_roll_neutral;
+ estimator_theta = atan2(Int_val(front), Int_val(top)) - ins_pitch_neutral;
+ sim_air_speed = Double_val(air_speed);
+ return Val_unit;
+}
+
diff --git a/sw/airborne/arch/sim/sim_ap.c b/sw/airborne/arch/sim/sim_ap.c
index 9a8d211881..f344d8f74f 100644
--- a/sw/airborne/arch/sim/sim_ap.c
+++ b/sw/airborne/arch/sim/sim_ap.c
@@ -117,3 +117,9 @@ value set_datalink_message(value s) {
dl_parse_msg();
return Val_unit;
}
+
+/** Required by electrical */
+void adc_buf_channel(void* a __attribute__ ((unused)),
+ void* b __attribute__ ((unused)),
+ void* c __attribute__ ((unused))) {
+}
diff --git a/sw/airborne/arch/sim/sim_ir.c b/sw/airborne/arch/sim/sim_ir.c
index 1ec7884faf..deb6a6b59d 100644
--- a/sw/airborne/arch/sim/sim_ir.c
+++ b/sw/airborne/arch/sim/sim_ir.c
@@ -16,23 +16,19 @@ float sim_air_speed;
void ir_gain_calib(void) {
}
-value set_ir(value roll __attribute__ ((unused)),
- value front __attribute__ ((unused)),
- value top __attribute__ ((unused)),
- value air_speed
- ) {
+value set_ir_and_airspeed(
+ value roll __attribute__ ((unused)),
+ value front __attribute__ ((unused)),
+ value top __attribute__ ((unused)),
+ value air_speed
+ ) {
// INFRARED_TELEMETRY : Stupid hack to use with modules
-#if defined USE_INFRARED || USE_INFRARED_TELEMETRY
+//#if defined USE_INFRARED || USE_INFRARED_TELEMETRY
infrared.roll = Int_val(roll);
infrared.pitch = Int_val(front);
infrared.top = Int_val(top);
-#endif
+//#endif
sim_air_speed = Double_val(air_speed);
return Val_unit;
}
-/** Required by infrared.c:ir_init() */
-void adc_buf_channel(void* a __attribute__ ((unused)),
- void* b __attribute__ ((unused)),
- void* c __attribute__ ((unused))) {
-}
diff --git a/sw/airborne/arch/sim/subsystems/imu/imu_b2_arch.c b/sw/airborne/arch/sim/subsystems/imu/imu_b2_arch.c
index f583072488..a8035a810f 100644
--- a/sw/airborne/arch/sim/subsystems/imu/imu_b2_arch.c
+++ b/sw/airborne/arch/sim/subsystems/imu/imu_b2_arch.c
@@ -47,11 +47,11 @@ void imu_feed_gyro_accel(void) {
void imu_feed_mag(void) {
-#if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2001
- ms2001_values[IMU_MAG_X_CHAN] = sensors.mag.value.x;
- ms2001_values[IMU_MAG_Y_CHAN] = sensors.mag.value.y;
- ms2001_values[IMU_MAG_Z_CHAN] = sensors.mag.value.z;
- ms2001_status = MS2001_DATA_AVAILABLE;
+#if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2100
+ ms2100_values[IMU_MAG_X_CHAN] = sensors.mag.value.x;
+ ms2100_values[IMU_MAG_Y_CHAN] = sensors.mag.value.y;
+ ms2100_values[IMU_MAG_Z_CHAN] = sensors.mag.value.z;
+ ms2100_status = MS2100_DATA_AVAILABLE;
#elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_AMI601
ami601_values[IMU_MAG_X_CHAN] = sensors.mag.value.x;
ami601_values[IMU_MAG_Y_CHAN] = sensors.mag.value.y;
diff --git a/sw/airborne/arch/sim/subsystems/settings_arch.c b/sw/airborne/arch/sim/subsystems/settings_arch.c
new file mode 100644
index 0000000000..1ed8a6a601
--- /dev/null
+++ b/sw/airborne/arch/sim/subsystems/settings_arch.c
@@ -0,0 +1,14 @@
+#include "subsystems/settings.h"
+
+
+int32_t persistent_write(uint32_t ptr, uint32_t size) {
+ ptr=ptr;
+ size=size;
+ return 0;
+}
+
+int32_t persistent_read(uint32_t ptr, uint32_t size) {
+ ptr=ptr;
+ size=size;
+ return 0;
+}
diff --git a/sw/airborne/arch/stm32/led_hw.h b/sw/airborne/arch/stm32/led_hw.h
index 133e070dd7..397781939f 100644
--- a/sw/airborne/arch/stm32/led_hw.h
+++ b/sw/airborne/arch/stm32/led_hw.h
@@ -42,10 +42,12 @@
#define _LED_GPIO_CLK(i) i
#define _LED_GPIO(i) i
#define _LED_GPIO_PIN(i) i
+#define _LED_AFIO_REMAP(i) i
#define LED_GPIO_CLK(i) _LED_GPIO_CLK(LED_ ## i ## _GPIO_CLK)
#define LED_GPIO(i) _LED_GPIO(LED_ ## i ## _GPIO)
#define LED_GPIO_PIN(i) _LED_GPIO_PIN(LED_ ## i ## _GPIO_PIN)
+#define LED_AFIO_REMAP(i) _LED_AFIO_REMAP(LED_ ## i ## _AFIO_REMAP)
/* set pin as output */
#define LED_INIT(i) { \
@@ -55,10 +57,11 @@
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; \
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; \
GPIO_Init(LED_GPIO(i), &GPIO_InitStructure); \
+ LED_AFIO_REMAP(i); \
}
-#define LED_ON(i) { LED_GPIO(i)->BRR = LED_GPIO_PIN(i);}
-#define LED_OFF(i) {LED_GPIO(i)->BSRR = LED_GPIO_PIN(i);}
+#define LED_ON(i) {LED_GPIO(i)->BSRR = LED_GPIO_PIN(i);}
+#define LED_OFF(i) { LED_GPIO(i)->BRR = LED_GPIO_PIN(i);}
#define LED_TOGGLE(i) { LED_GPIO(i)->ODR ^= LED_GPIO_PIN(i);}
#define LED_PERIODIC() {}
diff --git a/sw/airborne/arch/stm32/mcu_arch.c b/sw/airborne/arch/stm32/mcu_arch.c
index eb6760b1c6..494d252322 100644
--- a/sw/airborne/arch/stm32/mcu_arch.c
+++ b/sw/airborne/arch/stm32/mcu_arch.c
@@ -24,18 +24,26 @@
#include "mcu.h"
+#include BOARD_CONFIG
#include
#include
#include
#include
#include
+#ifdef USE_OPENCM3
+#include
+#endif
-#include BOARD_CONFIG
void mcu_arch_init(void) {
-
+#ifdef USE_OPENCM3
+ rcc_clock_setup_in_hse_12mhz_out_72mhz();
+ NVIC_SetVectorTable(NVIC_VectTab_FLASH, 0x0);
+ return;
+#endif
#ifdef HSE_TYPE_EXT_CLK
+#warning Using external clock
/* Setup the microcontroller system.
* Initialize the Embedded Flash Interface,
* initialize the PLL and update the SystemFrequency variable.
@@ -43,7 +51,7 @@ void mcu_arch_init(void) {
/* RCC system reset(for debug purpose) */
RCC_DeInit();
/* Enable HSE with external clock ( HSE_Bypass ) */
- RCC_HSEConfig( RCC_HSE_Bypass );
+ RCC_HSEConfig( STM32_RCC_MODE );
/* Wait till HSE is ready */
ErrorStatus HSEStartUpStatus = RCC_WaitForHSEStartUp();
if (HSEStartUpStatus != SUCCESS) {
@@ -62,7 +70,7 @@ void mcu_arch_init(void) {
/* PCLK1 = HCLK/2 */
RCC_PCLK1Config(RCC_HCLK_Div2);
/* PLLCLK = 8MHz * 9 = 72 MHz */
- RCC_PLLConfig(RCC_PLLSource_HSE_Div1, RCC_PLLMul_9);
+ RCC_PLLConfig(RCC_PLLSource_HSE_Div1, STM32_PLL_MULT);
/* Enable PLL */
RCC_PLLCmd(ENABLE);
/* Wait till PLL is ready */
@@ -73,10 +81,18 @@ void mcu_arch_init(void) {
while(RCC_GetSYSCLKSource() != 0x08) {}
}
#else /* HSE_TYPE_EXT_CLK */
+#warning Using normal system clock setup
SystemInit();
#endif /* HSE_TYPE_EXT_CLK */
/* Set the Vector Table base location at 0x08000000 */
NVIC_SetVectorTable(NVIC_VectTab_FLASH, 0x0);
+#ifdef STM32_FORCE_ALL_CLOCK_ON
+ RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB |
+ RCC_APB2Periph_GPIOC | RCC_APB2Periph_GPIOD |
+ RCC_APB2Periph_GPIOE | RCC_APB2Periph_AFIO, ENABLE);
+#endif
+
+
}
diff --git a/sw/airborne/arch/stm32/mcu_periph/adc_arch.c b/sw/airborne/arch/stm32/mcu_periph/adc_arch.c
index 8ff6734e31..114ede55b7 100644
--- a/sw/airborne/arch/stm32/mcu_periph/adc_arch.c
+++ b/sw/airborne/arch/stm32/mcu_periph/adc_arch.c
@@ -129,12 +129,12 @@ static inline void adc_init_irq( void );
*/
#ifdef USE_AD1
#ifndef ADC1_GPIO_INIT
-#define ADC1_GPIO_INIT(gpio) { \
+#define ADC1_GPIO_INIT(gpio) { \
(gpio).GPIO_Pin = GPIO_Pin_1 | GPIO_Pin_0; \
- (gpio).GPIO_Mode = GPIO_Mode_AIN; \
- GPIO_Init(GPIOB, (&gpio)); \
+ (gpio).GPIO_Mode = GPIO_Mode_AIN; \
+ GPIO_Init(GPIOB, (&gpio)); \
(gpio).GPIO_Pin = GPIO_Pin_5 | GPIO_Pin_3; \
- GPIO_Init(GPIOC, (&gpio)); \
+ GPIO_Init(GPIOC, (&gpio)); \
}
#endif // ADC1_GPIO_INIT
#endif // USE_AD1
@@ -145,13 +145,13 @@ static inline void adc_init_irq( void );
Uses the same GPIOs as ADC1 (lisa specific).
*/
#ifdef USE_AD2
-#define ADC2_GPIO_INIT(gpio) { \
+#define ADC2_GPIO_INIT(gpio) { \
(gpio).GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1; \
- (gpio).GPIO_Mode = GPIO_Mode_AIN; \
- GPIO_Init(GPIOB, (&gpio)); \
+ (gpio).GPIO_Mode = GPIO_Mode_AIN; \
+ GPIO_Init(GPIOB, (&gpio)); \
(gpio).GPIO_Pin = GPIO_Pin_3 | GPIO_Pin_5; \
- GPIO_Init(GPIOC, (&gpio)); \
-}
+ GPIO_Init(GPIOC, (&gpio)); \
+ }
#ifndef ADC2_GPIO_INIT
#define ADC2_GPIO_INIT(gpio) { }
#endif // ADC2_GPIO_INIT
@@ -362,10 +362,10 @@ static inline void adc_init_single(ADC_TypeDef * adc_t,
void adc_init( void ) {
/* initialize buffer pointers with 0 (not set).
- buffer null pointers will be ignored in interrupt
- handler, which is important as there are no
- buffers registered at the time the ADC trigger
- interrupt is enabled.
+ buffer null pointers will be ignored in interrupt
+ handler, which is important as there are no
+ buffers registered at the time the ADC trigger
+ interrupt is enabled.
*/
uint8_t channel;
#ifdef USE_AD1
@@ -382,14 +382,12 @@ void adc_init( void ) {
adc_injected_channels[1] = ADC_InjectedChannel_2;
adc_injected_channels[2] = ADC_InjectedChannel_3;
adc_injected_channels[3] = ADC_InjectedChannel_4;
- // TODO: Channel selection could be configured
- // using defines.
- adc_channel_map[0] = ADC_Channel_8;
- adc_channel_map[1] = ADC_Channel_9;
- // adc_channel_map[2] = ADC_Channel_13;
+ adc_channel_map[0] = BOARD_ADC_CHANNEL_1;
+ adc_channel_map[1] = BOARD_ADC_CHANNEL_2;
// FIXME for now we get battery voltage this way
- adc_channel_map[2] = ADC_Channel_0;
- adc_channel_map[3] = ADC_Channel_15;
+ // adc_channel_map[2] = BOARD_ADC_CHANNEL_3;
+ adc_channel_map[2] = BOARD_ADC_CHANNEL_3;
+ adc_channel_map[3] = BOARD_ADC_CHANNEL_4;
adc_init_rcc();
adc_init_irq();
diff --git a/sw/airborne/arch/stm32/mcu_periph/can_arch.c b/sw/airborne/arch/stm32/mcu_periph/can_arch.c
index 2af91534c1..002e0968cf 100644
--- a/sw/airborne/arch/stm32/mcu_periph/can_arch.c
+++ b/sw/airborne/arch/stm32/mcu_periph/can_arch.c
@@ -25,6 +25,7 @@
#include
#include
+#include "mcu_periph/can_arch.h"
#include "mcu_periph/can.h"
#include
diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c
index 7a922aeabb..195970cc20 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c
@@ -7,8 +7,418 @@
static void start_transaction(struct i2c_periph* p);
+static inline void end_of_transaction(struct i2c_periph *p);
+static inline void i2c_hard_reset(struct i2c_periph *p);
+static inline void i2c_reset_init(struct i2c_periph *p);
+
+#define I2C_BUSY 0x20
+
+#ifdef DEBUG_I2C
+#define SPURIOUS_INTERRUPT(_status, _event) { while(1); }
+#define OUT_OF_SYNC_STATE_MACHINE(_status, _event) { while(1); }
+#else
+#define SPURIOUS_INTERRUPT(_status, _event) {}
+#define OUT_OF_SYNC_STATE_MACHINE(_status, _event) {}
+#endif
+
+#ifdef USE_I2C1
+static I2C_InitTypeDef I2C1_InitStruct = {
+ .I2C_Mode = I2C_Mode_I2C,
+ .I2C_DutyCycle = I2C_DutyCycle_2,
+ .I2C_OwnAddress1 = 0x00,
+ .I2C_Ack = I2C_Ack_Enable,
+ .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
+ .I2C_ClockSpeed = 200000
+};
+#endif
+
+#ifdef USE_I2C2
+static I2C_InitTypeDef I2C2_InitStruct = {
+ .I2C_Mode = I2C_Mode_I2C,
+ .I2C_DutyCycle = I2C_DutyCycle_2,
+ .I2C_OwnAddress1 = 0x00,
+ .I2C_Ack = I2C_Ack_Enable,
+ .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
+ .I2C_ClockSpeed = 300000
+};
+#endif
+
+static inline void i2c_delay(void)
+{
+ for (__IO int j = 0; j < 50; j++);
+}
+
+static inline void i2c_apply_config(struct i2c_periph *p)
+{
+ I2C_Init(p->reg_addr, p->init_struct);
+}
+
+static inline void end_of_transaction(struct i2c_periph *p)
+{
+ p->trans_extract_idx++;
+ if (p->trans_extract_idx >= I2C_TRANSACTION_QUEUE_LEN)
+ p->trans_extract_idx = 0;
+ /* if we have no more transaction to process, stop here */
+ if (p->trans_extract_idx == p->trans_insert_idx)
+ p->status = I2CIdle;
+ /* if not, start next transaction */
+ else
+ start_transaction(p);
+}
+
+static inline void abort_and_reset(struct i2c_periph *p) {
+ struct i2c_transaction* trans = p->trans[p->trans_extract_idx];
+ trans->status = I2CTransFailed;
+ I2C_ITConfig(p->reg_addr, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE);
+ i2c_hard_reset(p);
+ I2C_ITConfig(p->reg_addr, I2C_IT_ERR, ENABLE);
+ end_of_transaction(p);
+}
+
+#ifdef USE_I2C2
+static inline void on_status_start_requested(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event);
+static inline void on_status_addr_wr_sent(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event);
+static inline void on_status_sending_byte(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event);
+static inline void on_status_stop_requested(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event);
+static inline void on_status_addr_rd_sent(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event);
+static inline void on_status_reading_byte(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event);
+static inline void on_status_reading_last_byte(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event);
+static inline void on_status_restart_requested(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event);
+
+/*
+ * Start Requested
+ *
+ */
+static inline void on_status_start_requested(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) {
+ if (event & I2C_FLAG_SB) {
+ if(trans->type == I2CTransRx) {
+ I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Receiver);
+ periph->status = I2CAddrRdSent;
+ }
+ else {
+ I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Transmitter);
+ periph->status = I2CAddrWrSent;
+ }
+ }
+ else
+ SPURIOUS_INTERRUPT(I2CStartRequested, event);
+}
+
+/*
+ * Addr WR sent
+ *
+ */
+static inline void on_status_addr_wr_sent(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) {
+ if ((event & I2C_FLAG_ADDR) && (event & I2C_FLAG_TRA)) {
+ I2C_SendData(periph->reg_addr, trans->buf[0]);
+ if (trans->len_w > 1) {
+ I2C_SendData(periph->reg_addr, trans->buf[1]);
+ periph->idx_buf = 2;
+ I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, ENABLE);
+ periph->status = I2CSendingByte;
+ }
+ else {
+ periph->idx_buf = 1;
+ if (trans->type == I2CTransTx) {
+ I2C_GenerateSTOP(periph->reg_addr, ENABLE);
+ periph->status = I2CStopRequested;
+ }
+ else {
+ I2C_GenerateSTART(periph->reg_addr, ENABLE);
+ periph->status = I2CRestartRequested;
+ }
+ }
+ }
+ else
+ SPURIOUS_INTERRUPT(I2CAddrWrSent, event);
+}
+
+/*
+ * Sending Byte
+ *
+ */
+static inline void on_status_sending_byte(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) {
+ I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
+ if (event & I2C_FLAG_TXE) {
+ if (periph->idx_buf < trans->len_w) {
+ I2C_SendData(periph->reg_addr, trans->buf[periph->idx_buf]);
+ periph->idx_buf++;
+ }
+ else {
+ I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, DISABLE);
+ if (trans->type == I2CTransTx) {
+ I2C_GenerateSTOP(periph->reg_addr, ENABLE);
+ /* Make sure that the STOP bit is cleared by Hardware */
+ static __IO uint8_t counter = 0;
+ while ((regs->CR1 & 0x200) == 0x200) {
+ counter++;
+ if (counter > 100) break;
+ }
+ periph->status = I2CStopRequested;
+ }
+ else {
+ I2C_GenerateSTART(periph->reg_addr, ENABLE);
+ periph->status = I2CRestartRequested;
+ }
+ }
+ }
+ else
+ SPURIOUS_INTERRUPT(I2CSendingByte, event);
+}
+
+/*
+ * Stop Requested
+ *
+ */
+static inline void on_status_stop_requested(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) {
+ /* bummer.... */
+ if (event & I2C_FLAG_RXNE) {
+ uint8_t read_byte = I2C_ReceiveData(periph->reg_addr);
+ if (periph->idx_buf < trans->len_r) {
+ trans->buf[periph->idx_buf] = read_byte;
+ }
+ }
+ I2C_ITConfig(periph->reg_addr, I2C_IT_EVT|I2C_IT_BUF, DISABLE); // should only need to disable evt, buf already disabled
+ trans->status = I2CTransSuccess;
+ end_of_transaction(periph);
+}
+
+/*
+ * Addr RD sent
+ *
+ */
+static inline void on_status_addr_rd_sent(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) {
+ I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
+
+ if ((event & I2C_FLAG_ADDR) && !(event & I2C_FLAG_TRA)) {
+ periph->idx_buf = 0;
+ if(trans->len_r == 1) { // If we're going to read only one byte
+ I2C_AcknowledgeConfig(periph->reg_addr, DISABLE); // make sure it's gonna be nacked
+ I2C_GenerateSTOP(periph->reg_addr, ENABLE); // and followed by a stop
+ /* Make sure that the STOP bit is cleared by Hardware */
+ static __IO uint8_t counter = 0;
+ while ((regs->CR1 & 0x200) == 0x200) {
+ counter++;
+ if (counter > 100) break;
+ }
+ periph->status = I2CReadingLastByte; // and remember we did
+ }
+ else {
+ I2C_AcknowledgeConfig(periph->reg_addr, ENABLE); // if it's more than one byte, ack it
+ I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, ENABLE);
+ periph->status = I2CReadingByte; // and remember we did
+ }
+ }
+ else
+ SPURIOUS_INTERRUPT(I2CAddrRdSent, event);
+}
+/*
+ * Reading byte
+ *
+ */
+static inline void on_status_reading_byte(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) {
+ I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
+ if (event & I2C_FLAG_RXNE) {
+ uint8_t read_byte = I2C_ReceiveData(periph->reg_addr);
+ if (periph->idx_buf < trans->len_r) {
+ trans->buf[periph->idx_buf] = read_byte;
+ periph->idx_buf++;
+ if (periph->idx_buf >= trans->len_r-1) { // We're reading our last byte
+ I2C_AcknowledgeConfig(periph->reg_addr, DISABLE); // give them a nack once it's done
+ I2C_GenerateSTOP(periph->reg_addr, ENABLE); // and follow with a stop
+ /* Make sure that the STOP bit is cleared by Hardware */
+ static __IO uint8_t counter = 0;
+ while ((regs->CR1 & 0x200) == 0x200) {
+ counter++;
+ if (counter > 100) break;
+ }
+ periph->status = I2CStopRequested; // remember we already trigered the stop
+ }
+ } // else { something very wrong has happened }
+ }
+ else
+ SPURIOUS_INTERRUPT(I2CReadingByte, event);
+}
+
+/*
+ * Reading last byte
+ *
+ */
+static inline void on_status_reading_last_byte(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) {
+ if (event & I2C_FLAG_BTF) {
+ uint8_t read_byte = I2C_ReceiveData(periph->reg_addr);
+ trans->buf[periph->idx_buf] = read_byte;
+ I2C_GenerateSTOP(periph->reg_addr, ENABLE);
+ periph->status = I2CStopRequested;
+ }
+ else if (event & I2C_FLAG_RXNE) { // should really be BTF ?
+ uint8_t read_byte = I2C_ReceiveData(periph->reg_addr);
+ trans->buf[periph->idx_buf] = read_byte;
+ periph->status = I2CStopRequested;
+ }
+ else
+ SPURIOUS_INTERRUPT(I2CReadingLastByte, event);
+}
+
+/*
+ * Restart requested
+ *
+ */
+static inline void on_status_restart_requested(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) {
+ if (event & I2C_FLAG_SB) {
+ I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Receiver);
+ periph->status = I2CAddrRdSent;
+ }
+}
+
+
+
+static inline void i2c_event(struct i2c_periph *p, uint32_t event)
+{
+ struct i2c_transaction* trans = p->trans[p->trans_extract_idx];
+ switch (p->status) {
+ case I2CStartRequested:
+ on_status_start_requested(p, trans, event);
+ break;
+ case I2CAddrWrSent:
+ on_status_addr_wr_sent(p, trans, event);
+ break;
+ case I2CSendingByte:
+ on_status_sending_byte(p, trans, event);
+ break;
+ case I2CStopRequested:
+ on_status_stop_requested(p, trans, event);
+ break;
+ case I2CAddrRdSent:
+ on_status_addr_rd_sent(p, trans, event);
+ break;
+ case I2CReadingByte:
+ on_status_reading_byte(p, trans, event);
+ break;
+ case I2CReadingLastByte:
+ on_status_reading_last_byte(p, trans, event);
+ break;
+ case I2CRestartRequested:
+ on_status_restart_requested(p, trans, event);
+ break;
+ default:
+ OUT_OF_SYNC_STATE_MACHINE(p->status, event);
+ break;
+ }
+}
+
+static inline void i2c_error(struct i2c_periph *p)
+{
+ p->errors->er_irq_cnt;
+ if (I2C_GetITStatus(p->reg_addr, I2C_IT_AF)) { /* Acknowledge failure */
+ p->errors->ack_fail_cnt++;
+ I2C_ClearITPendingBit(p->reg_addr, I2C_IT_AF);
+ }
+ if (I2C_GetITStatus(p->reg_addr, I2C_IT_BERR)) { /* Misplaced Start or Stop condition */
+ p->errors->miss_start_stop_cnt++;
+ I2C_ClearITPendingBit(p->reg_addr, I2C_IT_BERR);
+ }
+ if (I2C_GetITStatus(p->reg_addr, I2C_IT_ARLO)) { /* Arbitration lost */
+ p->errors->arb_lost_cnt++;
+ I2C_ClearITPendingBit(p->reg_addr, I2C_IT_ARLO);
+ // I2C_AcknowledgeConfig(I2C2, DISABLE);
+ // uint8_t dummy __attribute__ ((unused)) = I2C_ReceiveData(I2C2);
+ // I2C_GenerateSTOP(I2C2, ENABLE);
+ }
+ if (I2C_GetITStatus(p->reg_addr, I2C_IT_OVR)) { /* Overrun/Underrun */
+ p->errors->over_under_cnt++;
+ I2C_ClearITPendingBit(p->reg_addr, I2C_IT_OVR);
+ }
+ if (I2C_GetITStatus(p->reg_addr, I2C_IT_PECERR)) { /* PEC Error in reception */
+ p->errors->pec_recep_cnt++;
+ I2C_ClearITPendingBit(p->reg_addr, I2C_IT_PECERR);
+ }
+ if (I2C_GetITStatus(p->reg_addr, I2C_IT_TIMEOUT)) { /* Timeout or Tlow error */
+ p->errors->timeout_tlow_cnt++;
+ I2C_ClearITPendingBit(p->reg_addr, I2C_IT_TIMEOUT);
+ }
+ if (I2C_GetITStatus(p->reg_addr, I2C_IT_SMBALERT)) { /* SMBus alert */
+ p->errors->smbus_alert_cnt++;
+ I2C_ClearITPendingBit(p->reg_addr, I2C_IT_SMBALERT);
+ }
+
+ abort_and_reset(p);
+}
+
+
+static inline void i2c_hard_reset(struct i2c_periph *p)
+{
+ I2C_TypeDef *regs = (I2C_TypeDef *) p->reg_addr;
+
+ I2C_DeInit(p->reg_addr);
+
+ GPIO_InitTypeDef GPIO_InitStructure;
+ GPIO_InitStructure.GPIO_Pin = p->scl_pin | p->sda_pin;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;
+ GPIO_SetBits(GPIOB, p->scl_pin | p->sda_pin);
+ GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+ while(GPIO_ReadInputDataBit(GPIOB, p->sda_pin) == Bit_RESET) {
+ // Raise SCL, wait until SCL is high (in case of clock stretching)
+ GPIO_SetBits(GPIOB, p->scl_pin);
+ while (GPIO_ReadInputDataBit(GPIOB, p->scl_pin) == Bit_RESET);
+ i2c_delay();
+
+ // Lower SCL, wait
+ GPIO_ResetBits(GPIOB, p->scl_pin);
+ i2c_delay();
+
+ // Raise SCL, wait
+ GPIO_SetBits(GPIOB, p->scl_pin);
+ i2c_delay();
+ }
+
+ // Generate a start condition followed by a stop condition
+ GPIO_SetBits(GPIOB, p->scl_pin);
+ i2c_delay();
+ GPIO_ResetBits(GPIOB, p->sda_pin);
+ i2c_delay();
+ GPIO_ResetBits(GPIOB, p->sda_pin);
+ i2c_delay();
+
+ // Raise both SCL and SDA and wait for SCL high (in case of clock stretching)
+ GPIO_SetBits(GPIOB, p->scl_pin | p->sda_pin);
+ while (GPIO_ReadInputDataBit(GPIOB, p->scl_pin) == Bit_RESET);
+
+ // Wait for SDA to be high
+ while (GPIO_ReadInputDataBit(GPIOB, p->sda_pin) != Bit_SET);
+
+ // SCL and SDA should be high at this point, bus should be free
+ // Return the GPIO pins to the alternate function
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
+ GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+ I2C_DeInit(p->reg_addr);
+
+ i2c_apply_config(p);
+
+ if (regs->SR2 & I2C_BUSY) {
+ // Reset the I2C block
+ I2C_SoftwareResetCmd(p->reg_addr, ENABLE);
+ I2C_SoftwareResetCmd(p->reg_addr, DISABLE);
+ }
+}
+
+static inline void i2c_reset_init(struct i2c_periph *p)
+{
+ // Reset bus and configure GPIO pins
+ i2c_hard_reset(p);
+
+ // enable peripheral
+ I2C_Cmd(p->reg_addr, ENABLE);
+
+ // enable error interrupts
+ I2C_ITConfig(p->reg_addr, I2C_IT_ERR, ENABLE);
+}
+#endif
#ifdef USE_I2C1
@@ -16,29 +426,6 @@ struct i2c_errors i2c1_errors;
#include "my_debug_servo.h"
-#define I2C1_APPLY_CONFIG() { \
- I2C_InitTypeDef I2C_InitStructure; \
- I2C_InitStructure.I2C_Mode = I2C_Mode_I2C; \
- I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2; \
- I2C_InitStructure.I2C_OwnAddress1 = 0x00; \
- I2C_InitStructure.I2C_Ack = I2C_Ack_Enable; \
- I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; \
- I2C_InitStructure.I2C_ClockSpeed = 200000; \
- I2C_Init(I2C1, &I2C_InitStructure); \
- }
-
-#define I2C1_END_OF_TRANSACTION() { \
- i2c1.trans_extract_idx++; \
- if (i2c1.trans_extract_idx>=I2C_TRANSACTION_QUEUE_LEN) \
- i2c1.trans_extract_idx = 0; \
- /* if we have no more transaction to process, stop here */ \
- if (i2c1.trans_extract_idx == i2c1.trans_insert_idx) \
- i2c1.status = I2CIdle; \
- /* if not, start next transaction */ \
- else \
- start_transaction(&i2c1); \
- }
-
#define I2C1_ABORT_AND_RESET() { \
struct i2c_transaction* trans2 = i2c1.trans[i2c1.trans_extract_idx]; \
trans2->status = I2CTransFailed; \
@@ -46,9 +433,9 @@ struct i2c_errors i2c1_errors;
I2C_Cmd(I2C1, DISABLE); \
I2C_DeInit(I2C1); \
I2C_Cmd(I2C1, ENABLE); \
- I2C1_APPLY_CONFIG(); \
+ i2c_apply_config(&i2c1); \
I2C_ITConfig(I2C1, I2C_IT_ERR, ENABLE); \
- I2C1_END_OF_TRANSACTION(); \
+ end_of_transaction(&i2c1); \
}
//
@@ -64,6 +451,10 @@ struct i2c_errors i2c1_errors;
void i2c1_hw_init(void) {
i2c1.reg_addr = I2C1;
+ i2c1.init_struct = &I2C1_InitStruct;
+ i2c1.scl_pin = GPIO_Pin_6;
+ i2c1.sda_pin = GPIO_Pin_7;
+ i2c1.errors = &i2c1_errors;
/* zeros error counter */
ZEROS_ERR_COUNTER(i2c1_errors);
@@ -106,7 +497,7 @@ void i2c1_hw_init(void) {
/* I2C Peripheral Enable */
I2C_Cmd(I2C1, ENABLE);
/* Apply I2C configuration after enabling it */
- I2C1_APPLY_CONFIG();
+ i2c_apply_config(&i2c1);
/* Enable I2C1 error interrupts */
I2C_ITConfig(I2C1, I2C_IT_ERR, ENABLE);
@@ -144,7 +535,6 @@ void i2c1_ev_irq_handler(void) {
/* Test on I2C1 EV8 and clear it */
case I2C_EVENT_MASTER_BYTE_TRANSMITTING: /* Without BTF, EV8 */
- // DEBUG_S5_TOGGLE();
if(i2c1.idx_buf < trans->len_w) {
I2C_SendData(I2C1, trans->buf[i2c1.idx_buf]);
i2c1.idx_buf++;
@@ -157,7 +547,6 @@ void i2c1_ev_irq_handler(void) {
break;
case I2C_EVENT_MASTER_BYTE_TRANSMITTED: /* With BTF EV8-2 */
- // DEBUG_S6_TOGGLE();
if(i2c1.idx_buf < trans->len_w) {
I2C_SendData(I2C1, trans->buf[i2c1.idx_buf]);
i2c1.idx_buf++;
@@ -165,7 +554,7 @@ void i2c1_ev_irq_handler(void) {
else {
trans->status = I2CTransSuccess;
I2C_ITConfig(I2C1, I2C_IT_EVT, DISABLE);
- I2C1_END_OF_TRANSACTION();
+ end_of_transaction(&i2c1);
}
// while (I2C_GetFlagStatus(I2C1, I2C_FLAG_MSL));
break;
@@ -174,7 +563,6 @@ void i2c1_ev_irq_handler(void) {
i2c1_errors.unexpected_event_cnt++;
i2c1_errors.last_unexpected_event = event;
// spurious Interrupt
- // DEBUG_S2_TOGGLE();
// I have already had I2C_EVENT_SLAVE_STOP_DETECTED ( 0x10 )
// let's clear that by restarting I2C
// if (event == I2C_EVENT_SLAVE_STOP_DETECTED) {
@@ -243,24 +631,13 @@ struct i2c_errors i2c2_errors;
#include "my_debug_servo.h"
-#define I2C2_APPLY_CONFIG() { \
- \
- I2C_InitTypeDef I2C_InitStructure= { \
- .I2C_Mode = I2C_Mode_I2C, \
- .I2C_DutyCycle = I2C_DutyCycle_2, \
- .I2C_OwnAddress1 = 0x00, \
- .I2C_Ack = I2C_Ack_Enable, \
- .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, \
- .I2C_ClockSpeed = 300000 \
- }; \
- I2C_Init(I2C2, &I2C_InitStructure); \
- \
- }
-
-
void i2c2_hw_init(void) {
i2c2.reg_addr = I2C2;
+ i2c2.init_struct = &I2C2_InitStruct;
+ i2c2.scl_pin = GPIO_Pin_10;
+ i2c2.sda_pin = GPIO_Pin_11;
+ i2c2.errors = &i2c2_errors;
/* zeros error counter */
ZEROS_ERR_COUNTER(i2c2_errors);
@@ -291,419 +668,21 @@ void i2c2_hw_init(void) {
/* Enable GPIOB clock */
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
- /* Configure I2C2 pins: SCL and SDA -----------------------------------------*/
- GPIO_InitTypeDef GPIO_InitStructure;
- GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10 | GPIO_Pin_11;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
- GPIO_Init(GPIOB, &GPIO_InitStructure);
-
- /* I2C Peripheral Enable ----------------------------------------------------*/
- I2C_Cmd(I2C2, ENABLE);
-
- /* Apply I2C configuration after enabling it */
- I2C2_APPLY_CONFIG();
-
- /* Enable I2C2 error interrupts ---------------------------------------------*/
- I2C_ITConfig(I2C2, I2C_IT_ERR, ENABLE);
-
- // DEBUG_SERVO1_INIT();
- // DEBUG_SERVO2_INIT();
+ // Reset and initialize I2C HW
+ i2c_reset_init(&i2c2);
}
-static inline void on_status_start_requested(struct i2c_transaction* trans, uint32_t event);
-static inline void on_status_addr_wr_sent(struct i2c_transaction* trans, uint32_t event);
-static inline void on_status_sending_byte(struct i2c_transaction* trans, uint32_t event);
-//static inline void on_status_sending_last_byte(struct i2c_transaction* trans, uint32_t event);
-static inline void on_status_stop_requested(struct i2c_transaction* trans, uint32_t event);
-static inline void on_status_addr_rd_sent(struct i2c_transaction* trans, uint32_t event);
-static inline void on_status_reading_byte(struct i2c_transaction* trans, uint32_t event);
-static inline void on_status_reading_last_byte(struct i2c_transaction* trans, uint32_t event);
-static inline void on_status_restart_requested(struct i2c_transaction* trans, uint32_t event);
-
-#ifdef DEBUG_I2C
-#define SPURIOUS_INTERRUPT(_status, _event) { while(1); }
-#define OUT_OF_SYNC_STATE_MACHINE(_status, _event) { while(1); }
-#else
-#define SPURIOUS_INTERRUPT(_status, _event) {}
-#define OUT_OF_SYNC_STATE_MACHINE(_status, _event) {}
-#endif
-
-
-#define I2C2_END_OF_TRANSACTION() { \
- i2c2.trans_extract_idx++; \
- if (i2c2.trans_extract_idx>=I2C_TRANSACTION_QUEUE_LEN) \
- i2c2.trans_extract_idx = 0; \
- /* if we have no more transaction to process, stop here */ \
- if (i2c2.trans_extract_idx == i2c2.trans_insert_idx) \
- i2c2.status = I2CIdle; \
- /* if not, start next transaction */ \
- else \
- start_transaction(&i2c2); \
- }
-
-#define I2C2_ABORT_AND_RESET() { \
- struct i2c_transaction* trans = i2c2.trans[i2c2.trans_extract_idx]; \
- trans->status = I2CTransFailed; \
- I2C_ITConfig(I2C2, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE); \
- I2C_ClearITPendingBit(I2C2, 0xFF); \
- I2C_Cmd(I2C2, DISABLE); \
- I2C_DeInit(I2C2); \
- I2C2_APPLY_CONFIG(); \
- I2C_Cmd(I2C2, ENABLE); \
- /* do something to unstuck the bus */ \
- GPIO_InitTypeDef GPIO_InitStructure; \
- GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10; \
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz; \
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; \
- GPIO_Init(GPIOB, &GPIO_InitStructure); \
- for (__IO int i = 0; i < 10; i++) {\
- for (__IO int j = 0; j < 50; j++); \
- GPIOB->BSRR = GPIO_Pin_10; \
- for (__IO int j = 0; j < 50; j++); \
- GPIOB->BRR = GPIO_Pin_10; \
- } \
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD; \
- GPIO_Init(GPIOB, &GPIO_InitStructure); \
- I2C_Cmd(I2C2, DISABLE); \
- I2C_DeInit(I2C2); \
- I2C2_APPLY_CONFIG(); \
- I2C_Cmd(I2C2, ENABLE); \
- I2C_ClearITPendingBit(I2C2, 0xFF); \
- I2C_ITConfig(I2C2, I2C_IT_ERR, ENABLE); \
- I2C2_END_OF_TRANSACTION(); \
- }
-
-
-
-/*
- * Start Requested
- *
- */
-static inline void on_status_start_requested(struct i2c_transaction* trans, uint32_t event) {
- if (event & I2C_FLAG_SB) {
- if(trans->type == I2CTransRx) {
- I2C_Send7bitAddress(I2C2, trans->slave_addr, I2C_Direction_Receiver);
- i2c2.status = I2CAddrRdSent;
- }
- else {
- I2C_Send7bitAddress(I2C2, trans->slave_addr, I2C_Direction_Transmitter);
- i2c2.status = I2CAddrWrSent;
- }
- }
- else
- SPURIOUS_INTERRUPT(I2CStartRequested, event);
-}
-
-/*
- * Addr WR sent
- *
- */
-static inline void on_status_addr_wr_sent(struct i2c_transaction* trans, uint32_t event) {
- if ((event & I2C_FLAG_ADDR) && (event & I2C_FLAG_TRA)) {
- I2C_SendData(I2C2, trans->buf[0]);
- if (trans->len_w > 1) {
- I2C_SendData(I2C2, trans->buf[1]);
- i2c2.idx_buf = 2;
- I2C_ITConfig(I2C2, I2C_IT_BUF, ENABLE);
- i2c2.status = I2CSendingByte;
- }
- else {
- i2c2.idx_buf = 1;
- if (trans->type == I2CTransTx) {
- I2C_GenerateSTOP(I2C2, ENABLE);
- i2c2.status = I2CStopRequested;
- }
- else {
- I2C_GenerateSTART(I2C2, ENABLE);
- i2c2.status = I2CRestartRequested;
- }
- }
- }
- else
- SPURIOUS_INTERRUPT(I2CAddrWrSent, event);
-}
-
-/*
- * Sending Byte
- *
- */
-static inline void on_status_sending_byte(struct i2c_transaction* trans, uint32_t event) {
- if (event & I2C_FLAG_TXE) {
- if (i2c2.idx_buf < trans->len_w) {
- I2C_SendData(I2C2, trans->buf[i2c2.idx_buf]);
- i2c2.idx_buf++;
- }
- else {
- I2C_ITConfig(I2C2, I2C_IT_BUF, DISABLE);
- if (trans->type == I2CTransTx) {
- I2C_GenerateSTOP(I2C2, ENABLE);
- /* Make sure that the STOP bit is cleared by Hardware */
- while ((I2C2->CR1&0x200) == 0x200);
- i2c2.status = I2CStopRequested;
- }
- else {
- I2C_GenerateSTART(I2C2, ENABLE);
- i2c2.status = I2CRestartRequested;
- }
- }
- }
- else
- SPURIOUS_INTERRUPT(I2CSendingByte, event);
-}
-
-#if 0
-/*
- * Sending last byte
- *
- */
-static inline void on_status_sending_last_byte(struct i2c_transaction* trans, uint32_t event) {
- if (event & I2C_FLAG_TXE) { // should really be BTF as we're supposed to have disabled buf it already
- struct i2c_transaction* trans = i2c2.trans[i2c2.trans_extract_idx];
- if (trans->type == I2CTransTx) {
- I2C_GenerateSTOP(I2C2, ENABLE);
- i2c2.status = I2CStopRequested;
- }
- else {
- I2C_GenerateSTART(I2C2, ENABLE);
- i2c2.status = I2CRestartRequested;
- }
- // I2C_ITConfig(I2C2, I2C_IT_BUF, DISABLE);
- }
- else
- SPURIOUS_INTERRUPT(I2CSendingLastByte, event);
-}
-#endif
-
-
-/*
- * Stop Requested
- *
- */
-static inline void on_status_stop_requested(struct i2c_transaction* trans, uint32_t event) {
- /* bummer.... */
- if (event & I2C_FLAG_RXNE) {
- uint8_t read_byte = I2C_ReceiveData(I2C2);
- if (i2c2.idx_buf < trans->len_r) {
- trans->buf[i2c2.idx_buf] = read_byte;
- }
- }
- I2C_ITConfig(I2C2, I2C_IT_EVT|I2C_IT_BUF, DISABLE); // should only need to disable evt, buf already disabled
- trans->status = I2CTransSuccess;
- I2C2_END_OF_TRANSACTION();
-}
-
-/*
- * Addr RD sent
- *
- */
-static inline void on_status_addr_rd_sent(struct i2c_transaction* trans, uint32_t event) {
- if ((event & I2C_FLAG_ADDR) && !(event & I2C_FLAG_TRA)) {
- i2c2.idx_buf = 0;
- if(trans->len_r == 1) { // If we're going to read only one byte
- I2C_AcknowledgeConfig(I2C2, DISABLE); // make sure it's gonna be nacked
- I2C_GenerateSTOP(I2C2, ENABLE); // and followed by a stop
- /* Make sure that the STOP bit is cleared by Hardware */
- while ((I2C2->CR1&0x200) == 0x200);
- i2c2.status = I2CReadingLastByte; // and remember we did
- }
- else {
- I2C_AcknowledgeConfig(I2C2, ENABLE); // if it's more than one byte, ack it
- I2C_ITConfig(I2C2, I2C_IT_BUF, ENABLE);
- i2c2.status = I2CReadingByte; // and remember we did
- }
- }
- else
- SPURIOUS_INTERRUPT(I2CAddrRdSent, event);
-}
-
-
-/*
- * Reading byte
- *
- */
-static inline void on_status_reading_byte(struct i2c_transaction* trans, uint32_t event) {
- if (event & I2C_FLAG_RXNE) {
- uint8_t read_byte = I2C_ReceiveData(I2C2);
- if (i2c2.idx_buf < trans->len_r) {
- trans->buf[i2c2.idx_buf] = read_byte;
- i2c2.idx_buf++;
- if (i2c2.idx_buf >= trans->len_r-1) { // We're reading our last byte
- I2C_AcknowledgeConfig(I2C2, DISABLE); // give them a nack once it's done
- I2C_GenerateSTOP(I2C2, ENABLE); // and follow with a stop
- /* Make sure that the STOP bit is cleared by Hardware */
- while ((I2C2->CR1&0x200) == 0x200);
- i2c2.status = I2CStopRequested; // remember we already trigered the stop
- }
- } // else { something very wrong has happened }
- }
- else
- SPURIOUS_INTERRUPT(I2CReadingByte, event);
-}
-
-/*
- * Reading last byte
- *
- */
-static inline void on_status_reading_last_byte(struct i2c_transaction* trans, uint32_t event) {
- if (event & I2C_FLAG_BTF) {
- uint8_t read_byte = I2C_ReceiveData(I2C2);
- trans->buf[i2c2.idx_buf] = read_byte;
- I2C_GenerateSTOP(I2C2, ENABLE);
- i2c2.status = I2CStopRequested;
- }
- else if (event & I2C_FLAG_RXNE) { // should really be BTF ?
- uint8_t read_byte = I2C_ReceiveData(I2C2);
- trans->buf[i2c2.idx_buf] = read_byte;
- i2c2.status = I2CStopRequested;
- }
- else
- SPURIOUS_INTERRUPT(I2CReadingLastByte, event);
-}
-
-/*
- * Restart requested
- *
- */
-static inline void on_status_restart_requested(struct i2c_transaction* trans, uint32_t event) {
- // DEBUG_S6_ON();
- if (event & I2C_FLAG_SB) {
- // DEBUG_S2_ON();
- I2C_Send7bitAddress(I2C2, trans->slave_addr, I2C_Direction_Receiver);
- i2c2.status = I2CAddrRdSent;
- // DEBUG_S2_OFF();
- }
-
- if (event & I2C_FLAG_BTF) {
- // DEBUG_S5_ON();
- // DEBUG_S5_OFF();
- }
-
- if (event & I2C_FLAG_TXE) {
- // DEBUG_S3_ON();
- // DEBUG_S3_OFF();
- }
-
- // if (event & I2C_FLAG_TXE) {
- // DEBUG_S2_ON();
- // DEBUG_S2_OFF();
- // }
-
-
- // else if (event & I2C_FLAG_TXE) {
- // i2c2.status = I2CReadingByte;
- // }
- // else
- // SPURIOUS_INTERRUPT(I2CRestartRequested, event);
- // DEBUG_S6_OFF();
-}
-
void i2c2_ev_irq_handler(void) {
- // DEBUG_S4_ON();
uint32_t event = I2C_GetLastEvent(I2C2);
- struct i2c_transaction* trans = i2c2.trans[i2c2.trans_extract_idx];
- //#if 0
- // if (i2c2_errors.irq_cnt < 16) {
- // i2c2_errors.event_chain[i2c2_errors.irq_cnt] = event;
- // i2c2_errors.status_chain[i2c2_errors.irq_cnt] = i2c2.status;
- // i2c2_errors.irq_cnt++;
- // } else { while (1);}
- //#endif
- switch (i2c2.status) {
- case I2CStartRequested:
- on_status_start_requested(trans, event);
- break;
- case I2CAddrWrSent:
- on_status_addr_wr_sent(trans, event);
- break;
- case I2CSendingByte:
- // DEBUG_S4_ON();
- on_status_sending_byte(trans, event);
- // DEBUG_S4_OFF();
- break;
-#if 0
- case I2CSendingLastByte:
- // DEBUG_S5_ON();
- on_status_sending_last_byte(trans, event);
- // DEBUG_S5_OFF();
- break;
-#endif
- case I2CStopRequested:
- // DEBUG_S1_ON();
- on_status_stop_requested(trans, event);
- // DEBUG_S1_OFF();
- break;
- case I2CAddrRdSent:
- on_status_addr_rd_sent(trans, event);
- break;
- case I2CReadingByte:
- // DEBUG_S2_ON();
- on_status_reading_byte(trans, event);
- // DEBUG_S2_OFF();
- break;
- case I2CReadingLastByte:
- // DEBUG_S5_ON();
- on_status_reading_last_byte(trans, event);
- // DEBUG_S5_OFF();
- break;
- case I2CRestartRequested:
- // DEBUG_S5_ON();
- on_status_restart_requested(trans, event);
- // DEBUG_S5_OFF();
- break;
- default:
- OUT_OF_SYNC_STATE_MACHINE(i2c2.status, event);
- break;
- }
- // DEBUG_S4_OFF();
+ i2c_event(&i2c2, event);
}
-
void i2c2_er_irq_handler(void) {
- // DEBUG_S5_ON();
- i2c2_errors.er_irq_cnt;
- if (I2C_GetITStatus(I2C2, I2C_IT_AF)) { /* Acknowledge failure */
- i2c2_errors.ack_fail_cnt++;
- I2C_ClearITPendingBit(I2C2, I2C_IT_AF);
- I2C_GenerateSTOP(I2C2, ENABLE);
- /* Make sure that the STOP bit is cleared by Hardware */
- while ((I2C2->CR1&0x200) == 0x200);
- }
- if (I2C_GetITStatus(I2C2, I2C_IT_BERR)) { /* Misplaced Start or Stop condition */
- i2c2_errors.miss_start_stop_cnt++;
- I2C_ClearITPendingBit(I2C2, I2C_IT_BERR);
- }
- if (I2C_GetITStatus(I2C2, I2C_IT_ARLO)) { /* Arbitration lost */
- i2c2_errors.arb_lost_cnt++;
- I2C_ClearITPendingBit(I2C2, I2C_IT_ARLO);
- // I2C_AcknowledgeConfig(I2C2, DISABLE);
- // uint8_t dummy __attribute__ ((unused)) = I2C_ReceiveData(I2C2);
- // I2C_GenerateSTOP(I2C2, ENABLE);
- }
- if (I2C_GetITStatus(I2C2, I2C_IT_OVR)) { /* Overrun/Underrun */
- i2c2_errors.over_under_cnt++;
- I2C_ClearITPendingBit(I2C2, I2C_IT_OVR);
- }
- if (I2C_GetITStatus(I2C2, I2C_IT_PECERR)) { /* PEC Error in reception */
- i2c2_errors.pec_recep_cnt++;
- I2C_ClearITPendingBit(I2C2, I2C_IT_PECERR);
- }
- if (I2C_GetITStatus(I2C2, I2C_IT_TIMEOUT)) { /* Timeout or Tlow error */
- i2c2_errors.timeout_tlow_cnt++;
- I2C_ClearITPendingBit(I2C2, I2C_IT_TIMEOUT);
- }
- if (I2C_GetITStatus(I2C2, I2C_IT_SMBALERT)) { /* SMBus alert */
- i2c2_errors.smbus_alert_cnt++;
- I2C_ClearITPendingBit(I2C2, I2C_IT_SMBALERT);
- }
-
- I2C2_ABORT_AND_RESET();
-
- // DEBUG_S5_OFF();
+ i2c_error(&i2c2);
}
diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.new.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.new.c
new file mode 100644
index 0000000000..59dfcf823a
--- /dev/null
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.new.c
@@ -0,0 +1,616 @@
+#include "mcu_periph/i2c.h"
+
+#include
+#include
+#include
+#include
+
+
+static void start_transaction(struct i2c_periph* p);
+static inline void end_of_transaction(struct i2c_periph *p);
+static inline void i2c_hard_reset(struct i2c_periph *p);
+static inline void i2c_reset_init(struct i2c_periph *p);
+
+#define I2C_BUSY 0x20
+
+#ifdef DEBUG_I2C
+#define SPURIOUS_INTERRUPT(_periph, _status, _event) { while(1); }
+#define OUT_OF_SYNC_STATE_MACHINE(_periph, _status, _event) { while(1); }
+#else
+//#define SPURIOUS_INTERRUPT(_periph, _status, _event) { periph->errors->unexpected_event_cnt++; abort_and_reset(_periph);}
+#define SPURIOUS_INTERRUPT(_periph, _status, _event) { if (_status == I2CAddrWrSent) abort_and_reset(_periph);}
+#define OUT_OF_SYNC_STATE_MACHINE(_periph, _status, _event) { abort_and_reset(_periph);}
+#endif
+
+#ifdef USE_I2C1
+static I2C_InitTypeDef I2C1_InitStruct = {
+ .I2C_Mode = I2C_Mode_I2C,
+ .I2C_DutyCycle = I2C_DutyCycle_2,
+ .I2C_OwnAddress1 = 0x00,
+ .I2C_Ack = I2C_Ack_Enable,
+ .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
+ .I2C_ClockSpeed = 200000
+};
+#endif
+
+#ifdef USE_I2C2
+static I2C_InitTypeDef I2C2_InitStruct = {
+ .I2C_Mode = I2C_Mode_I2C,
+ .I2C_DutyCycle = I2C_DutyCycle_2,
+ .I2C_OwnAddress1 = 0x00,
+ .I2C_Ack = I2C_Ack_Enable,
+ .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
+ .I2C_ClockSpeed = 300000
+};
+#endif
+
+static inline void i2c_delay(void)
+{
+ for (__IO int j = 0; j < 50; j++);
+}
+
+static inline void i2c_apply_config(struct i2c_periph *p)
+{
+ I2C_Init(p->reg_addr, p->init_struct);
+}
+
+static inline void end_of_transaction(struct i2c_periph *p)
+{
+ p->trans_extract_idx++;
+ if (p->trans_extract_idx >= I2C_TRANSACTION_QUEUE_LEN)
+ p->trans_extract_idx = 0;
+ /* if we have no more transaction to process, stop here */
+ if (p->trans_extract_idx == p->trans_insert_idx)
+ p->status = I2CIdle;
+ /* if not, start next transaction */
+ else
+ start_transaction(p);
+}
+
+static inline void abort_and_reset(struct i2c_periph *p) {
+ struct i2c_transaction* trans = p->trans[p->trans_extract_idx];
+ trans->status = I2CTransFailed;
+ I2C_ITConfig(p->reg_addr, I2C_IT_EVT | I2C_IT_BUF | I2C_IT_ERR, DISABLE);
+ i2c_hard_reset(p);
+ I2C_ITConfig(p->reg_addr, I2C_IT_ERR, ENABLE);
+ end_of_transaction(p);
+}
+
+#ifdef USE_I2C2
+static inline void on_status_start_requested(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event);
+static inline void on_status_addr_wr_sent(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event);
+static inline void on_status_sending_byte(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event);
+static inline void on_status_stop_requested(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event);
+static inline void on_status_addr_rd_sent(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event);
+static inline void on_status_reading_byte(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event);
+static inline void on_status_reading_last_byte(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event);
+static inline void on_status_restart_requested(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event);
+
+/*
+ * Start Requested
+ *
+ */
+static inline void on_status_start_requested(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) {
+ if (event & I2C_FLAG_SB) {
+ if(trans->type == I2CTransRx) {
+ I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Receiver);
+ periph->status = I2CAddrRdSent;
+ }
+ else {
+ I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Transmitter);
+ periph->status = I2CAddrWrSent;
+ }
+ }
+ // else
+ // SPURIOUS_INTERRUPT(periph, I2CStartRequested, event);
+ // FIXME: this one seems to get called all the time with mkk controllers
+}
+
+/*
+ * Addr WR sent
+ *
+ */
+static inline void on_status_addr_wr_sent(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) {
+ if ((event & I2C_FLAG_ADDR) && (event & I2C_FLAG_TRA)) {
+ I2C_SendData(periph->reg_addr, trans->buf[0]);
+ if (trans->len_w > 1) {
+ I2C_SendData(periph->reg_addr, trans->buf[1]);
+ periph->idx_buf = 2;
+ I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, ENABLE);
+ periph->status = I2CSendingByte;
+ }
+ else {
+ periph->idx_buf = 1;
+ if (trans->type == I2CTransTx) {
+ I2C_GenerateSTOP(periph->reg_addr, ENABLE);
+ periph->status = I2CStopRequested;
+ }
+ else {
+ I2C_GenerateSTART(periph->reg_addr, ENABLE);
+ periph->status = I2CRestartRequested;
+ }
+ }
+ }
+ else {
+ SPURIOUS_INTERRUPT(periph, I2CAddrWrSent, event);
+ // FIXME: this was where the code would break with mkk controllers on april 10 2011
+ // now have SPURIOUS_INTERRUPT call abort_and_reset
+ }
+}
+
+/*
+ * Sending Byte
+ *
+ */
+static inline void on_status_sending_byte(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) {
+ I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
+ if (event & I2C_FLAG_TXE) {
+ if (periph->idx_buf < trans->len_w) {
+ I2C_SendData(periph->reg_addr, trans->buf[periph->idx_buf]);
+ periph->idx_buf++;
+ }
+ else {
+ I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, DISABLE);
+ if (trans->type == I2CTransTx) {
+ I2C_GenerateSTOP(periph->reg_addr, ENABLE);
+ /* Make sure that the STOP bit is cleared by Hardware */
+ static __IO uint8_t counter = 0;
+ while ((regs->CR1 & 0x200) == 0x200) {
+ counter++;
+ if (counter > 100) break;
+ }
+ periph->status = I2CStopRequested;
+ }
+ else {
+ I2C_GenerateSTART(periph->reg_addr, ENABLE);
+ periph->status = I2CRestartRequested;
+ }
+ }
+ }
+ else
+ SPURIOUS_INTERRUPT(periph, I2CSendingByte, event);
+}
+
+/*
+ * Stop Requested
+ *
+ */
+static inline void on_status_stop_requested(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) {
+ /* bummer.... */
+ if (event & I2C_FLAG_RXNE) {
+ uint8_t read_byte = I2C_ReceiveData(periph->reg_addr);
+ if (periph->idx_buf < trans->len_r) {
+ trans->buf[periph->idx_buf] = read_byte;
+ }
+ }
+ I2C_ITConfig(periph->reg_addr, I2C_IT_EVT|I2C_IT_BUF, DISABLE); // should only need to disable evt, buf already disabled
+ trans->status = I2CTransSuccess;
+ end_of_transaction(periph);
+}
+
+/*
+ * Addr RD sent
+ *
+ */
+static inline void on_status_addr_rd_sent(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) {
+ I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
+
+ if ((event & I2C_FLAG_ADDR) && !(event & I2C_FLAG_TRA)) {
+ periph->idx_buf = 0;
+ if(trans->len_r == 1) { // If we're going to read only one byte
+ I2C_AcknowledgeConfig(periph->reg_addr, DISABLE); // make sure it's gonna be nacked
+ I2C_GenerateSTOP(periph->reg_addr, ENABLE); // and followed by a stop
+ /* Make sure that the STOP bit is cleared by Hardware */
+ static __IO uint8_t counter = 0;
+ while ((regs->CR1 & 0x200) == 0x200) {
+ counter++;
+ if (counter > 100) break;
+ }
+ periph->status = I2CReadingLastByte; // and remember we did
+ }
+ else {
+ I2C_AcknowledgeConfig(periph->reg_addr, ENABLE); // if it's more than one byte, ack it
+ I2C_ITConfig(periph->reg_addr, I2C_IT_BUF, ENABLE);
+ periph->status = I2CReadingByte; // and remember we did
+ }
+ }
+ else
+ SPURIOUS_INTERRUPT(periph, I2CAddrRdSent, event);
+}
+
+
+/*
+ * Reading byte
+ *
+ */
+static inline void on_status_reading_byte(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) {
+ I2C_TypeDef *regs = (I2C_TypeDef *) periph->reg_addr;
+ if (event & I2C_FLAG_RXNE) {
+ uint8_t read_byte = I2C_ReceiveData(periph->reg_addr);
+ if (periph->idx_buf < trans->len_r) {
+ trans->buf[periph->idx_buf] = read_byte;
+ periph->idx_buf++;
+ if (periph->idx_buf >= trans->len_r-1) { // We're reading our last byte
+ I2C_AcknowledgeConfig(periph->reg_addr, DISABLE); // give them a nack once it's done
+ I2C_GenerateSTOP(periph->reg_addr, ENABLE); // and follow with a stop
+ /* Make sure that the STOP bit is cleared by Hardware */
+ static __IO uint8_t counter = 0;
+ while ((regs->CR1 & 0x200) == 0x200) {
+ counter++;
+ if (counter > 100) break;
+ }
+ periph->status = I2CStopRequested; // remember we already trigered the stop
+ }
+ } // else { something very wrong has happened }
+ }
+ else
+ SPURIOUS_INTERRUPT(periph, I2CReadingByte, event);
+}
+
+/*
+ * Reading last byte
+ *
+ */
+static inline void on_status_reading_last_byte(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) {
+ if (event & I2C_FLAG_BTF) {
+ uint8_t read_byte = I2C_ReceiveData(periph->reg_addr);
+ trans->buf[periph->idx_buf] = read_byte;
+ I2C_GenerateSTOP(periph->reg_addr, ENABLE);
+ periph->status = I2CStopRequested;
+ }
+ else if (event & I2C_FLAG_RXNE) { // should really be BTF ?
+ uint8_t read_byte = I2C_ReceiveData(periph->reg_addr);
+ trans->buf[periph->idx_buf] = read_byte;
+ periph->status = I2CStopRequested;
+ }
+ else
+ SPURIOUS_INTERRUPT(periph, I2CReadingLastByte, event);
+}
+
+/*
+ * Restart requested
+ *
+ */
+static inline void on_status_restart_requested(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event) {
+ if (event & I2C_FLAG_SB) {
+ I2C_Send7bitAddress(periph->reg_addr, trans->slave_addr, I2C_Direction_Receiver);
+ periph->status = I2CAddrRdSent;
+ }
+}
+
+
+
+static inline void i2c_event(struct i2c_periph *p, uint32_t event)
+{
+ struct i2c_transaction* trans = p->trans[p->trans_extract_idx];
+ switch (p->status) {
+ case I2CStartRequested:
+ on_status_start_requested(p, trans, event);
+ break;
+ case I2CAddrWrSent:
+ on_status_addr_wr_sent(p, trans, event);
+ break;
+ case I2CSendingByte:
+ on_status_sending_byte(p, trans, event);
+ break;
+ case I2CStopRequested:
+ on_status_stop_requested(p, trans, event);
+ break;
+ case I2CAddrRdSent:
+ on_status_addr_rd_sent(p, trans, event);
+ break;
+ case I2CReadingByte:
+ on_status_reading_byte(p, trans, event);
+ break;
+ case I2CReadingLastByte:
+ on_status_reading_last_byte(p, trans, event);
+ break;
+ case I2CRestartRequested:
+ on_status_restart_requested(p, trans, event);
+ break;
+ default:
+ OUT_OF_SYNC_STATE_MACHINE(p, p->status, event);
+ break;
+ }
+}
+
+static inline void i2c_error(struct i2c_periph *p)
+{
+ p->errors->er_irq_cnt;
+ if (I2C_GetITStatus(p->reg_addr, I2C_IT_AF)) { /* Acknowledge failure */
+ p->errors->ack_fail_cnt++;
+ I2C_ClearITPendingBit(p->reg_addr, I2C_IT_AF);
+ }
+ if (I2C_GetITStatus(p->reg_addr, I2C_IT_BERR)) { /* Misplaced Start or Stop condition */
+ p->errors->miss_start_stop_cnt++;
+ I2C_ClearITPendingBit(p->reg_addr, I2C_IT_BERR);
+ }
+ if (I2C_GetITStatus(p->reg_addr, I2C_IT_ARLO)) { /* Arbitration lost */
+ p->errors->arb_lost_cnt++;
+ I2C_ClearITPendingBit(p->reg_addr, I2C_IT_ARLO);
+ // I2C_AcknowledgeConfig(I2C2, DISABLE);
+ // uint8_t dummy __attribute__ ((unused)) = I2C_ReceiveData(I2C2);
+ // I2C_GenerateSTOP(I2C2, ENABLE);
+ }
+ if (I2C_GetITStatus(p->reg_addr, I2C_IT_OVR)) { /* Overrun/Underrun */
+ p->errors->over_under_cnt++;
+ I2C_ClearITPendingBit(p->reg_addr, I2C_IT_OVR);
+ }
+ if (I2C_GetITStatus(p->reg_addr, I2C_IT_PECERR)) { /* PEC Error in reception */
+ p->errors->pec_recep_cnt++;
+ I2C_ClearITPendingBit(p->reg_addr, I2C_IT_PECERR);
+ }
+ if (I2C_GetITStatus(p->reg_addr, I2C_IT_TIMEOUT)) { /* Timeout or Tlow error */
+ p->errors->timeout_tlow_cnt++;
+ I2C_ClearITPendingBit(p->reg_addr, I2C_IT_TIMEOUT);
+ }
+ if (I2C_GetITStatus(p->reg_addr, I2C_IT_SMBALERT)) { /* SMBus alert */
+ p->errors->smbus_alert_cnt++;
+ I2C_ClearITPendingBit(p->reg_addr, I2C_IT_SMBALERT);
+ }
+
+ abort_and_reset(p);
+}
+
+
+static inline void i2c_hard_reset(struct i2c_periph *p)
+{
+ I2C_TypeDef *regs = (I2C_TypeDef *) p->reg_addr;
+
+ I2C_DeInit(p->reg_addr);
+
+ GPIO_InitTypeDef GPIO_InitStructure;
+ GPIO_InitStructure.GPIO_Pin = p->scl_pin | p->sda_pin;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;
+ GPIO_SetBits(GPIOB, p->scl_pin | p->sda_pin);
+ GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+ while(GPIO_ReadInputDataBit(GPIOB, p->sda_pin) == Bit_RESET) {
+ // Raise SCL, wait until SCL is high (in case of clock stretching)
+ GPIO_SetBits(GPIOB, p->scl_pin);
+ while (GPIO_ReadInputDataBit(GPIOB, p->scl_pin) == Bit_RESET);
+ i2c_delay();
+
+ // Lower SCL, wait
+ GPIO_ResetBits(GPIOB, p->scl_pin);
+ i2c_delay();
+
+ // Raise SCL, wait
+ GPIO_SetBits(GPIOB, p->scl_pin);
+ i2c_delay();
+ }
+
+ // Generate a start condition followed by a stop condition
+ GPIO_SetBits(GPIOB, p->scl_pin);
+ i2c_delay();
+ GPIO_ResetBits(GPIOB, p->sda_pin);
+ i2c_delay();
+ GPIO_ResetBits(GPIOB, p->sda_pin);
+ i2c_delay();
+
+ // Raise both SCL and SDA and wait for SCL high (in case of clock stretching)
+ GPIO_SetBits(GPIOB, p->scl_pin | p->sda_pin);
+ while (GPIO_ReadInputDataBit(GPIOB, p->scl_pin) == Bit_RESET);
+
+ // Wait for SDA to be high
+ while (GPIO_ReadInputDataBit(GPIOB, p->sda_pin) != Bit_SET);
+
+ // SCL and SDA should be high at this point, bus should be free
+ // Return the GPIO pins to the alternate function
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
+ GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+ I2C_DeInit(p->reg_addr);
+
+ i2c_apply_config(p);
+
+ if (regs->SR2 & I2C_BUSY) {
+ // Reset the I2C block
+ I2C_SoftwareResetCmd(p->reg_addr, ENABLE);
+ I2C_SoftwareResetCmd(p->reg_addr, DISABLE);
+ }
+}
+
+static inline void i2c_reset_init(struct i2c_periph *p)
+{
+ // Reset bus and configure GPIO pins
+ i2c_hard_reset(p);
+
+ // enable peripheral
+ I2C_Cmd(p->reg_addr, ENABLE);
+
+ // enable error interrupts
+ I2C_ITConfig(p->reg_addr, I2C_IT_ERR, ENABLE);
+}
+#endif /* USE_I2C2 */
+
+#ifdef USE_I2C1
+
+struct i2c_errors i2c1_errors;
+
+#include "my_debug_servo.h"
+
+void i2c1_hw_init(void) {
+
+ i2c1.reg_addr = I2C1;
+ i2c1.init_struct = &I2C1_InitStruct;
+ i2c1.scl_pin = GPIO_Pin_6;
+ i2c1.sda_pin = GPIO_Pin_7;
+ i2c1.errors = &i2c1_errors;
+
+ /* zeros error counter */
+ ZEROS_ERR_COUNTER(i2c1_errors);
+
+ /* reset peripheral to default state ( sometimes not achieved on reset :( ) */
+ I2C_DeInit(I2C1);
+
+ NVIC_PriorityGroupConfig(NVIC_PriorityGroup_0);
+ NVIC_InitTypeDef NVIC_InitStructure;
+
+ /* Configure and enable I2C1 event interrupt -------------------------------*/
+ NVIC_InitStructure.NVIC_IRQChannel = I2C1_EV_IRQn;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ NVIC_Init(&NVIC_InitStructure);
+
+ /* Configure and enable I2C1 err interrupt -------------------------------*/
+ NVIC_InitStructure.NVIC_IRQChannel = I2C1_ER_IRQn;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ NVIC_Init(&NVIC_InitStructure);
+
+ /* Enable peripheral clocks --------------------------------------------------*/
+ /* Enable I2C1 clock */
+ RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE);
+ /* Enable GPIOB clock */
+ RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
+
+ /* Configure I2C1 pins: SCL and SDA ------------------------------------------*/
+ GPIO_InitTypeDef GPIO_InitStructure;
+ GPIO_StructInit(&GPIO_InitStructure);
+ GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
+ GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+ /* I2C configuration ----------------------------------------------------------*/
+
+
+ /* Reset and initialize I2C HW */
+ i2c_reset_init(&i2c1);
+
+}
+
+
+void i2c1_ev_irq_handler(void) {
+
+ uint32_t event = I2C_GetLastEvent(I2C1);
+ i2c_event(&i2c1, event);
+
+}
+
+
+void i2c1_er_irq_handler(void) {
+ i2c_error(&i2c1);
+}
+
+#endif /* USE_I2C1 */
+
+
+
+
+
+#ifdef USE_I2C2
+
+// dec hex
+// 196609 30001 BUSY MSL | SB
+// 458882 70082 TRA BUSY MSL | TXE ADDR
+// 458884 70084 TRA BUSY MSL | TXE BTF
+// 196609 30001 BUSY MSL | SB
+// 196610 30002 BUSY MSL | ADDR
+//
+
+
+struct i2c_errors i2c2_errors;
+
+#include "my_debug_servo.h"
+
+void i2c2_hw_init(void) {
+
+ i2c2.reg_addr = I2C2;
+ i2c2.init_struct = &I2C2_InitStruct;
+ i2c2.scl_pin = GPIO_Pin_10;
+ i2c2.sda_pin = GPIO_Pin_11;
+ i2c2.errors = &i2c2_errors;
+
+ /* zeros error counter */
+ ZEROS_ERR_COUNTER(i2c2_errors);
+
+ /* reset peripheral to default state ( sometimes not achieved on reset :( ) */
+ I2C_DeInit(I2C2);
+
+ NVIC_PriorityGroupConfig(NVIC_PriorityGroup_0);
+ NVIC_InitTypeDef NVIC_InitStructure;
+
+ /* Configure and enable I2C2 event interrupt --------------------------------*/
+ NVIC_InitStructure.NVIC_IRQChannel = I2C2_EV_IRQn;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ NVIC_Init(&NVIC_InitStructure);
+
+ /* Configure and enable I2C2 err interrupt ----------------------------------*/
+ NVIC_InitStructure.NVIC_IRQChannel = I2C2_ER_IRQn;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ NVIC_Init(&NVIC_InitStructure);
+
+ /* Enable peripheral clocks -------------------------------------------------*/
+ /* Enable I2C2 clock */
+ RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE);
+ /* Enable GPIOB clock */
+ RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
+
+ // Reset and initialize I2C HW
+ i2c_reset_init(&i2c2);
+
+}
+
+
+
+
+void i2c2_ev_irq_handler(void) {
+ uint32_t event = I2C_GetLastEvent(I2C2);
+ i2c_event(&i2c2, event);
+}
+
+void i2c2_er_irq_handler(void) {
+ i2c_error(&i2c2);
+
+}
+
+#endif /* USE_I2C2 */
+
+
+
+bool_t i2c_idle(struct i2c_periph* p)
+{
+ return !I2C_GetFlagStatus(p->reg_addr, I2C_FLAG_BUSY);
+}
+
+bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t) {
+
+ uint8_t temp;
+ temp = p->trans_insert_idx + 1;
+ if (temp >= I2C_TRANSACTION_QUEUE_LEN) temp = 0;
+ if (temp == p->trans_extract_idx)
+ return FALSE; // queue full
+
+ t->status = I2CTransPending;
+
+
+ __disable_irq();
+ /* put transacation in queue */
+ p->trans[p->trans_insert_idx] = t;
+ p->trans_insert_idx = temp;
+
+ /* if peripheral is idle, start the transaction */
+ if (p->status == I2CIdle)
+ start_transaction(p);
+ /* else it will be started by the interrupt handler when the previous transactions completes */
+ __enable_irq();
+
+ return TRUE;
+}
+
+
+static void start_transaction(struct i2c_periph* p) {
+ p->idx_buf = 0;
+ p->status = I2CStartRequested;
+ I2C_ITConfig(p->reg_addr, I2C_IT_EVT, ENABLE);
+ I2C_GenerateSTART(p->reg_addr, ENABLE);
+}
diff --git a/sw/airborne/arch/stm32/mcu_periph/uart_arch.c b/sw/airborne/arch/stm32/mcu_periph/uart_arch.c
index 3edce0c1d1..e0406d9c85 100644
--- a/sw/airborne/arch/stm32/mcu_periph/uart_arch.c
+++ b/sw/airborne/arch/stm32/mcu_periph/uart_arch.c
@@ -28,6 +28,7 @@
#include
#include
#include "std.h"
+#include "pprz_baudrate.h"
#ifdef USE_UART1
@@ -42,6 +43,7 @@ uint8_t uart1_tx_buffer[UART1_TX_BUFFER_SIZE];
void uart1_init( void ) {
/* init RCC */
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);
+ RCC_APB2PeriphClockCmd(UART1_Periph, ENABLE);
/* Enable USART1 interrupts */
NVIC_InitTypeDef nvic;
@@ -74,6 +76,9 @@ void uart1_init( void ) {
USART_Init(USART1, &usart);
/* Enable USART1 Receive interrupts */
USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);
+
+ pprz_usart_set_baudrate(USART1, UART1_BAUD);
+
/* Enable the USART1 */
USART_Cmd(USART1, ENABLE);
@@ -165,6 +170,7 @@ uint8_t uart2_tx_buffer[UART2_TX_BUFFER_SIZE];
void uart2_init( void ) {
/* init RCC */
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE);
+ RCC_APB2PeriphClockCmd(UART2_Periph, ENABLE);
/* Enable USART2 interrupts */
NVIC_InitTypeDef nvic;
@@ -197,6 +203,9 @@ void uart2_init( void ) {
USART_Init(USART2, &usart);
/* Enable USART2 Receive interrupts */
USART_ITConfig(USART2, USART_IT_RXNE, ENABLE);
+
+ pprz_usart_set_baudrate(USART2, UART2_BAUD);
+
/* Enable the USART2 */
USART_Cmd(USART2, ENABLE);
@@ -287,6 +296,7 @@ void uart3_init( void ) {
/* init RCC */
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
+ RCC_APB2PeriphClockCmd(UART3_Periph, ENABLE);
/* Enable USART3 interrupts */
NVIC_InitTypeDef nvic;
@@ -320,6 +330,9 @@ void uart3_init( void ) {
USART_Init(USART3, &usart);
/* Enable USART3 Receive interrupts */
USART_ITConfig(USART3, USART_IT_RXNE, ENABLE);
+
+ pprz_usart_set_baudrate(USART3, UART3_BAUD);
+
/* Enable the USART3 */
USART_Cmd(USART3, ENABLE);
@@ -392,6 +405,128 @@ void usart3_irq_handler(void) {
#endif /* USE_UART3 */
+#ifdef USE_UART5
+
+volatile uint16_t uart5_rx_insert_idx, uart5_rx_extract_idx;
+uint8_t uart5_rx_buffer[UART5_RX_BUFFER_SIZE];
+
+volatile uint16_t uart5_tx_insert_idx, uart5_tx_extract_idx;
+volatile bool_t uart5_tx_running;
+uint8_t uart5_tx_buffer[UART5_TX_BUFFER_SIZE];
+
+void uart5_init( void ) {
+
+ /* init RCC */
+ RCC_APB1PeriphClockCmd(RCC_APB1Periph_UART5, ENABLE);
+ RCC_APB2PeriphClockCmd(UART5_PeriphTx, ENABLE);
+ RCC_APB2PeriphClockCmd(UART5_PeriphRx, ENABLE);
+
+ /* Enable UART5 interrupts */
+ NVIC_InitTypeDef nvic;
+ nvic.NVIC_IRQChannel = UART5_IRQn;
+ nvic.NVIC_IRQChannelPreemptionPriority = 2;
+ nvic.NVIC_IRQChannelSubPriority = 1;
+ nvic.NVIC_IRQChannelCmd = ENABLE;
+ NVIC_Init(&nvic);
+
+ /* Init GPIOS */
+ GPIO_InitTypeDef gpio;
+ /* GPIOC: GPIO_Pin_10 UART5 Tx push-pull */
+ gpio.GPIO_Pin = UART5_TxPin;
+ gpio.GPIO_Mode = GPIO_Mode_AF_PP;
+ gpio.GPIO_Speed = GPIO_Speed_50MHz;
+ GPIO_Init(UART5_TxPort, &gpio);
+ /* GPIOC: GPIO_Pin_11 UART5 Rx pin as floating input */
+ gpio.GPIO_Pin = UART5_RxPin;
+ gpio.GPIO_Mode = GPIO_Mode_IN_FLOATING;
+ GPIO_Init(UART5_RxPort, &gpio);
+
+ /* Configure UART5 */
+ USART_InitTypeDef usart;
+ usart.USART_BaudRate = UART5_BAUD;
+ usart.USART_WordLength = USART_WordLength_8b;
+ usart.USART_StopBits = USART_StopBits_1;
+ usart.USART_Parity = USART_Parity_No;
+ usart.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
+ usart.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
+ USART_Init(USART5, &usart);
+ /* Enable UART5 Receive interrupts */
+ USART_ITConfig(UART5, USART_IT_RXNE, ENABLE);
+
+ pprz_usart_set_baudrate(UART5, UART5_BAUD);
+
+ /* Enable the UART5 */
+ USART_Cmd(UART5, ENABLE);
+
+ // initialize the transmit data queue
+ uart5_tx_extract_idx = 0;
+ uart5_tx_insert_idx = 0;
+ uart5_tx_running = FALSE;
+
+ // initialize the receive data queuenn
+ uart5_rx_extract_idx = 0;
+ uart5_rx_insert_idx = 0;
+
+}
+
+void uart5_transmit( uint8_t data ) {
+
+ uint16_t temp = (uart5_tx_insert_idx + 1) % UART5_TX_BUFFER_SIZE;
+
+ if (temp == uart5_tx_extract_idx)
+ return; // no room
+
+ USART_ITConfig(USART5, USART_IT_TXE, DISABLE);
+
+ // check if in process of sending data
+ if (uart5_tx_running) { // yes, add to queue
+ uart5_tx_buffer[uart5_tx_insert_idx] = data;
+ uart5_tx_insert_idx = temp;
+ }
+ else { // no, set running flag and write to output register
+ uart5_tx_running = TRUE;
+ USART_SendData(USART5, data);
+ }
+ USART_ITConfig(USART5, USART_IT_TXE, ENABLE);
+
+}
+
+bool_t uart5_check_free_space( uint8_t len) {
+ int16_t space = uart5_tx_extract_idx - uart5_tx_insert_idx;
+ if (space <= 0)
+ space += UART5_TX_BUFFER_SIZE;
+ return (uint16_t)(space - 1) >= len;
+}
+
+
+void usart5_irq_handler(void) {
+
+ if(USART_GetITStatus(USART5, USART_IT_TXE) != RESET){
+ // check if more data to send
+ if (uart5_tx_insert_idx != uart5_tx_extract_idx) {
+ USART_SendData(USART5,uart5_tx_buffer[uart5_tx_extract_idx]);
+ uart5_tx_extract_idx++;
+ uart5_tx_extract_idx %= UART5_TX_BUFFER_SIZE;
+ }
+ else {
+ uart5_tx_running = FALSE; // clear running flag
+ USART_ITConfig(USART5, USART_IT_TXE, DISABLE);
+ }
+ }
+
+ if(USART_GetITStatus(USART5, USART_IT_RXNE) != RESET){
+ uint16_t temp = (uart5_rx_insert_idx + 1) % UART5_RX_BUFFER_SIZE;;
+ uart5_rx_buffer[uart5_rx_insert_idx] = USART_ReceiveData(USART5);
+ // check for more room in queue
+ if (temp != uart5_rx_extract_idx)
+ uart5_rx_insert_idx = temp; // update insert index
+ }
+
+}
+
+
+#endif /* USE_UART5 */
+
void uart_init( void )
{
#ifdef USE_UART1
@@ -403,6 +538,9 @@ void uart_init( void )
#ifdef USE_UART3
uart3_init();
#endif
+#ifdef USE_UART5
+ uart5_init();
+#endif
}
diff --git a/sw/airborne/arch/stm32/mcu_periph/uart_arch.h b/sw/airborne/arch/stm32/mcu_periph/uart_arch.h
index e281700dba..3d7113ef42 100644
--- a/sw/airborne/arch/stm32/mcu_periph/uart_arch.h
+++ b/sw/airborne/arch/stm32/mcu_periph/uart_arch.h
@@ -44,6 +44,7 @@
#define UART1_TxPin GPIO_Pin_9
#define UART2_TxPin GPIO_Pin_2
#define UART3_TxPin GPIO_Pin_10
+#define UART5_TxPin GPIO_Pin_12
#define UART1_RxPin GPIO_Pin_10
#define UART2_RxPin GPIO_Pin_3
@@ -53,6 +54,7 @@
#define UART1_TxPort GPIOA
#define UART2_TxPort GPIOA
#define UART3_TxPort GPIOC
+#define UART5_TxPort GPIOC
#define UART1_RxPort GPIOA
#define UART2_RxPort GPIOA
@@ -62,6 +64,13 @@
#define UART1_Periph RCC_APB2Periph_GPIOA
#define UART2_Periph RCC_APB2Periph_GPIOA
#define UART3_Periph RCC_APB2Periph_GPIOC
+#define UART5_PeriphTx RCC_APB2Periph_GPIOC
+#define UART5_PeriphRx RCC_APB2Periph_GPIOD
+
+/* this is unexpected the macros in spektrum_arch.c
+ didn't expect that rx and tx would be spilt over
+ two ports. As the spektrum code is only interested
+ in the rx pin we define this to be the Peripheral */
#define UART5_Periph RCC_APB2Periph_GPIOD
#define UART1_UartPeriph RCC_APB2Periph_USART1
@@ -180,6 +189,27 @@ extern uint8_t uart3_tx_buffer[UART3_TX_BUFFER_SIZE];
#endif /* USE_UART3 */
+#ifdef USE_UART5
+
+#define UART5_RX_BUFFER_SIZE 128
+#define UART5_TX_BUFFER_SIZE 128
+
+extern volatile uint16_t uart5_rx_insert_idx, uart5_rx_extract_idx;
+extern uint8_t uart5_rx_buffer[UART5_RX_BUFFER_SIZE];
+
+extern volatile uint16_t uart5_tx_insert_idx, uart5_tx_extract_idx;
+extern volatile bool_t uart5_tx_running;
+extern uint8_t uart5_tx_buffer[UART5_TX_BUFFER_SIZE];
+
+#define Uart5ChAvailable() (uart5_rx_insert_idx != uart5_rx_extract_idx)
+#define Uart5Getch() ({ \
+ uint8_t ret = uart5_rx_buffer[uart5_rx_extract_idx]; \
+ uart5_rx_extract_idx = (uart5_rx_extract_idx + 1)%UART5_RX_BUFFER_SIZE; \
+ ret; \
+ })
+
+#endif /* USE_UART5 */
+
void uart_init( void );
diff --git a/sw/airborne/arch/stm32/peripherals/ms2001_arch.c b/sw/airborne/arch/stm32/peripherals/ms2100_arch.c
similarity index 86%
rename from sw/airborne/arch/stm32/peripherals/ms2001_arch.c
rename to sw/airborne/arch/stm32/peripherals/ms2100_arch.c
index b9b91646e6..961ab71e53 100644
--- a/sw/airborne/arch/stm32/peripherals/ms2001_arch.c
+++ b/sw/airborne/arch/stm32/peripherals/ms2100_arch.c
@@ -21,7 +21,7 @@
* Boston, MA 02111-1307, USA.
*/
-#include "peripherals/ms2001.h"
+#include "peripherals/ms2100.h"
#include
#include
@@ -29,16 +29,16 @@
#include
#include
-uint8_t ms2001_cur_axe;
-int16_t ms2001_last_reading; // can't write in place because that stupid beast
+uint8_t ms2100_cur_axe;
+int16_t ms2100_last_reading; // can't write in place because that stupid beast
// stips stupid values once in a while that I need
// to filter - high time we get rid of this crap hardware
// and no, I checked with the logic analyzer, timing are
// within specs
-void ms2001_arch_init( void ) {
+void ms2100_arch_init( void ) {
- ms2001_cur_axe = 0;
+ ms2100_cur_axe = 0;
/* set mag SS and reset as output and assert them (SS on PC12 reset on PC13) ----*/
Ms2001Unselect();
@@ -58,7 +58,7 @@ void ms2001_arch_init( void ) {
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB, &GPIO_InitStructure);
-#ifdef MS2001_HANDLES_DMA_IRQ
+#ifdef MS2100_HANDLES_DMA_IRQ
/* Enable DMA1 channel4 IRQ Channel */
NVIC_InitTypeDef NVIC_init_structure_dma = {
.NVIC_IRQChannel = DMA1_Channel4_IRQn,
@@ -67,9 +67,9 @@ void ms2001_arch_init( void ) {
.NVIC_IRQChannelCmd = ENABLE
};
NVIC_Init(&NVIC_init_structure_dma);
-#endif /* MS2001_HANDLES_DMA_IRQ */
+#endif /* MS2100_HANDLES_DMA_IRQ */
-#ifdef MS2001_HANDLES_SPI_IRQ
+#ifdef MS2100_HANDLES_SPI_IRQ
NVIC_InitTypeDef NVIC_init_structure_spi = {
.NVIC_IRQChannel = SPI2_IRQn,
.NVIC_IRQChannelPreemptionPriority = 0,
@@ -77,19 +77,19 @@ void ms2001_arch_init( void ) {
.NVIC_IRQChannelCmd = ENABLE
};
NVIC_Init(&NVIC_init_structure_spi);
-#endif /* MS2001_HANDLES_SPI_IRQ */
+#endif /* MS2100_HANDLES_SPI_IRQ */
}
-#ifdef MS2001_HANDLES_SPI_IRQ
+#ifdef MS2100_HANDLES_SPI_IRQ
void spi2_irq_handler(void) {
Ms2001OnSpiIrq();
}
#endif
-#ifdef MS2001_HANDLES_DMA_IRQ
+#ifdef MS2100_HANDLES_DMA_IRQ
void dma1_c4_irq_handler(void) {
Ms2001OnDmaIrq();
}
-#endif /* MS2001_HANDLES_DMA_IRQ */
+#endif /* MS2100_HANDLES_DMA_IRQ */
diff --git a/sw/airborne/arch/stm32/peripherals/ms2001_arch.h b/sw/airborne/arch/stm32/peripherals/ms2100_arch.h
similarity index 84%
rename from sw/airborne/arch/stm32/peripherals/ms2001_arch.h
rename to sw/airborne/arch/stm32/peripherals/ms2100_arch.h
index 5008bc5f6c..c95a26ce84 100644
--- a/sw/airborne/arch/stm32/peripherals/ms2001_arch.h
+++ b/sw/airborne/arch/stm32/peripherals/ms2100_arch.h
@@ -1,5 +1,5 @@
-#ifndef MS2001_ARCH_H
-#define MS2001_ARCH_H
+#ifndef MS2100_ARCH_H
+#define MS2100_ARCH_H
/*
* $Id$
@@ -27,8 +27,8 @@
#include
#include
-extern uint8_t ms2001_cur_axe;
-extern int16_t ms2001_last_reading;
+extern uint8_t ms2100_cur_axe;
+extern int16_t ms2100_last_reading;
#define Ms2001Select() GPIOC->BRR = GPIO_Pin_12
#define Ms2001Unselect() GPIOC->BSRR = GPIO_Pin_12
@@ -42,10 +42,10 @@ extern int16_t ms2001_last_reading;
Ms2001Select(); \
__IO uint32_t nCount = 4;for(; nCount != 0; nCount--); \
Ms2001Reset(); \
- ms2001_status = MS2001_SENDING_REQ; \
+ ms2100_status = MS2100_SENDING_REQ; \
nCount = 4;for(; nCount != 0; nCount--); \
Ms2001Set(); \
- uint16_t ctl_byte = ((ms2001_cur_axe+1) | (MS2001_DIVISOR << 4)); \
+ uint16_t ctl_byte = ((ms2100_cur_axe+1) | (MS2100_DIVISOR << 4)); \
nCount = 20;for(; nCount != 0; nCount--); \
SPI_Cmd(SPI2, DISABLE); \
SPI_InitTypeDef SPI_InitStructure = { \
@@ -66,7 +66,7 @@ extern int16_t ms2001_last_reading;
}
#define Ms2001ReadRes() { \
- ms2001_status = MS2001_READING_RES; \
+ ms2100_status = MS2100_READING_RES; \
Ms2001Select(); \
SPI_Cmd(SPI2, DISABLE); \
SPI_InitTypeDef SPI_InitStructure = { \
@@ -88,7 +88,7 @@ extern int16_t ms2001_last_reading;
DMA_InitTypeDef DMA_InitStructure; \
DMA_DeInit(DMA1_Channel4); \
DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)(SPI2_BASE+0x0C); \
- DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)(&ms2001_last_reading); \
+ DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)(&ms2100_last_reading); \
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC; \
DMA_InitStructure.DMA_BufferSize = 1; \
DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; \
@@ -102,7 +102,7 @@ extern int16_t ms2001_last_reading;
/* SPI2_Tx_DMA_Channel configuration ------------------------------------*/ \
DMA_DeInit(DMA1_Channel5); \
DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)(SPI2_BASE+0x0C); \
- DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&ms2001_values; \
+ DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&ms2100_values; \
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST; \
DMA_InitStructure.DMA_Priority = DMA_Priority_Medium; \
DMA_Init(DMA1_Channel5, &DMA_InitStructure); \
@@ -123,33 +123,33 @@ extern int16_t ms2001_last_reading;
}
#define Ms2001OnDmaIrq() { \
- /* ASSERT((ms2001_status == MS2001_READING_RES), \
- * DEBUG_MS2001, MS2001_ERR_SPURIOUS_DMA_IRQ); \
+ /* ASSERT((ms2100_status == MS2100_READING_RES), \
+ * DEBUG_MS2100, MS2100_ERR_SPURIOUS_DMA_IRQ); \
*/ \
- if (abs(ms2001_last_reading) < 1000) \
- ms2001_values[ms2001_cur_axe] = ms2001_last_reading; \
+ if (abs(ms2100_last_reading) < 1000) \
+ ms2100_values[ms2100_cur_axe] = ms2100_last_reading; \
Ms2001Unselect(); \
- ms2001_cur_axe++; \
- if (ms2001_cur_axe > 2) { \
- ms2001_cur_axe = 0; \
- ms2001_status = MS2001_DATA_AVAILABLE; \
+ ms2100_cur_axe++; \
+ if (ms2100_cur_axe > 2) { \
+ ms2100_cur_axe = 0; \
+ ms2100_status = MS2100_DATA_AVAILABLE; \
} \
else \
- ms2001_status = MS2001_IDLE; \
+ ms2100_status = MS2100_IDLE; \
SPI_Cmd(SPI2, DISABLE); \
DMA_ITConfig(DMA1_Channel4, DMA_IT_TC, DISABLE); \
}
#define Ms2001OnSpiIrq() { \
- /* ASSERT((ms2001_status == MS2001_SENDING_REQ), \
- * DEBUG_MS2001, MS2001_ERR_SPURIOUS_SPI_IRQ); \
+ /* ASSERT((ms2100_status == MS2100_SENDING_REQ), \
+ * DEBUG_MS2100, MS2100_ERR_SPURIOUS_SPI_IRQ); \
*/ \
/* read unused control byte reply */ \
uint8_t foo __attribute__ ((unused)) = SPI_I2S_ReceiveData(SPI2); \
Ms2001Unselect(); \
- ms2001_status = MS2001_WAITING_EOC; \
+ ms2100_status = MS2100_WAITING_EOC; \
SPI_Cmd(SPI2, DISABLE); \
SPI_I2S_ITConfig(SPI2, SPI_I2S_IT_RXNE, DISABLE); \
}
-#endif /* MS2001_ARCH_H */
+#endif /* MS2100_ARCH_H */
diff --git a/sw/airborne/arch/stm32/pprz_baudrate.h b/sw/airborne/arch/stm32/pprz_baudrate.h
new file mode 100644
index 0000000000..bb4a7369a2
--- /dev/null
+++ b/sw/airborne/arch/stm32/pprz_baudrate.h
@@ -0,0 +1,13 @@
+#ifndef __PPRZ_BAUDRATE_H
+#define __PPRZ_BAUDRATE_H
+
+#include BOARD_CONFIG
+
+#ifdef USE_OPENCM3
+void usart_set_baudrate(void *usart, uint32_t baud);
+#define pprz_usart_set_baudrate(x, y) usart_set_baudrate(x, y)
+#else
+#define pprz_usart_set_baudrate(x, y) do { } while(0);
+#endif
+
+#endif
diff --git a/sw/airborne/arch/stm32/stm32f103rb_flash.ld b/sw/airborne/arch/stm32/stm32f103rb_flash.ld
index 7e1575e460..95181a8db5 100644
--- a/sw/airborne/arch/stm32/stm32f103rb_flash.ld
+++ b/sw/airborne/arch/stm32/stm32f103rb_flash.ld
@@ -25,7 +25,8 @@
MEMORY
{
RAM (xrw): ORIGIN = 0x20000000, LENGTH = 20K
- FLASH (rx) : ORIGIN = 0x8000000, LENGTH = 128K
+ /* last page (1k) flash for persistent settings */
+ FLASH (rx) : ORIGIN = 0x8000000, LENGTH = 127K
FLASHB1 (rx) : ORIGIN = 0x00000000, LENGTH = 0
}
diff --git a/sw/airborne/arch/stm32/stm32f103re_flash.ld b/sw/airborne/arch/stm32/stm32f103re_flash.ld
index 3df6455c4c..94fcdbdbd3 100644
--- a/sw/airborne/arch/stm32/stm32f103re_flash.ld
+++ b/sw/airborne/arch/stm32/stm32f103re_flash.ld
@@ -25,7 +25,8 @@
MEMORY
{
RAM (xrw): ORIGIN = 0x20000000, LENGTH = 64K
- FLASH (rx) : ORIGIN = 0x8000000, LENGTH = 512K
+ /* last page (2k) flash for persistent settings */
+ FLASH (rx) : ORIGIN = 0x8000000, LENGTH = 510K
FLASHB1 (rx) : ORIGIN = 0x00000000, LENGTH = 0
}
diff --git a/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.c b/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.c
index b3b5a94109..3715342009 100644
--- a/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.c
+++ b/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.c
@@ -21,11 +21,44 @@ void exti2_irq_handler(void);
/* accelerometer dma end of rx handler */
void dma1_c4_irq_handler(void);
+void imu_aspirin_arch_int_enable(void) {
+ NVIC_InitTypeDef NVIC_InitStructure;
+
+ NVIC_InitStructure.NVIC_IRQChannel = EXTI15_10_IRQn;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x0F;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x0F;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ NVIC_Init(&NVIC_InitStructure);
+
+ NVIC_InitStructure.NVIC_IRQChannel = EXTI2_IRQn;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x0F;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x0F;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ NVIC_Init(&NVIC_InitStructure);
+
+}
+
+void imu_aspirin_arch_int_disable(void) {
+ NVIC_InitTypeDef NVIC_InitStructure;
+
+ NVIC_InitStructure.NVIC_IRQChannel = EXTI15_10_IRQn;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x0F;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x0F;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;
+ NVIC_Init(&NVIC_InitStructure);
+
+ NVIC_InitStructure.NVIC_IRQChannel = EXTI2_IRQn;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x0F;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x0F;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;
+ NVIC_Init(&NVIC_InitStructure);
+
+}
+
void imu_aspirin_arch_init(void) {
GPIO_InitTypeDef GPIO_InitStructure;
EXTI_InitTypeDef EXTI_InitStructure;
- NVIC_InitTypeDef NVIC_InitStructure;
SPI_InitTypeDef SPI_InitStructure;
/* Set "mag ss" and "mag reset" as floating inputs ------------------------*/
@@ -57,13 +90,6 @@ void imu_aspirin_arch_init(void) {
EXTI_InitStructure.EXTI_LineCmd = ENABLE;
EXTI_Init(&EXTI_InitStructure);
- NVIC_InitStructure.NVIC_IRQChannel = EXTI15_10_IRQn;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x0F;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x0F;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_Init(&NVIC_InitStructure);
-
-
/* Accel */
/* set accel slave select as output and assert it ( on PB12) */
Adxl345Unselect();
@@ -87,12 +113,6 @@ void imu_aspirin_arch_init(void) {
EXTI_InitStructure.EXTI_LineCmd = ENABLE;
EXTI_Init(&EXTI_InitStructure);
- NVIC_InitStructure.NVIC_IRQChannel = EXTI2_IRQn;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x0F;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x0F;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_Init(&NVIC_InitStructure);
-
/* Enable SPI2 Periph clock -------------------------------------------------*/
RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2, ENABLE);
diff --git a/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.h b/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.h
index e40fed4777..4345e8b14c 100644
--- a/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.h
+++ b/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.h
@@ -6,6 +6,8 @@
#include "led.h"
extern void imu_aspirin_arch_init(void);
+extern void imu_aspirin_arch_int_enable(void);
+extern void imu_aspirin_arch_int_disable(void);
extern void adxl345_write_to_reg(uint8_t addr, uint8_t val);
extern void adxl345_clear_rx_buf(void);
extern void adxl345_start_reading_data(void);
diff --git a/sw/airborne/arch/stm32/subsystems/imu/imu_b2_arch.c b/sw/airborne/arch/stm32/subsystems/imu/imu_b2_arch.c
index 0ab657a5a0..0f229c3de7 100644
--- a/sw/airborne/arch/stm32/subsystems/imu/imu_b2_arch.c
+++ b/sw/airborne/arch/stm32/subsystems/imu/imu_b2_arch.c
@@ -91,12 +91,12 @@ void dma1_c4_irq_handler(void) {
case IMU_SSP_STA_BUSY_MAX1168:
Max1168OnDmaIrq();
SPI_Cmd(SPI2, DISABLE);
-#if IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2001
- if (ms2001_status == MS2001_IDLE) {
+#if IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2100
+ if (ms2100_status == MS2100_IDLE) {
Ms2001SendReq();
imu_ssp_status = IMU_SSP_STA_BUSY_MS2100;
}
- else if (ms2001_status == MS2001_WAITING_EOC && Ms2001HasEOC()) {
+ else if (ms2100_status == MS2100_WAITING_EOC && Ms2001HasEOC()) {
Ms2001ReadRes();
imu_ssp_status = IMU_SSP_STA_BUSY_MS2100;
}
@@ -105,7 +105,7 @@ void dma1_c4_irq_handler(void) {
imu_ssp_status = IMU_SSP_STA_IDLE;
break;
case IMU_SSP_STA_BUSY_MS2100:
-#if IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2001
+#if IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2100
Ms2001OnDmaIrq();
#endif
break;
@@ -117,7 +117,7 @@ void dma1_c4_irq_handler(void) {
void spi2_irq_handler(void) {
-#if IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2001
+#if IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2100
Ms2001OnSpiIrq();
#endif
}
diff --git a/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.c b/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.c
index 91b609b63d..ae8927d88f 100644
--- a/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.c
+++ b/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.c
@@ -30,6 +30,8 @@
#include "subsystems/radio_control.h"
#include "subsystems/radio_control/spektrum_arch.h"
#include "mcu_periph/uart.h"
+#include "pprz_baudrate.h"
+
#define SPEKTRUM_CHANNELS_PER_FRAME 7
@@ -85,7 +87,10 @@ typedef struct SpektrumStateStruct SpektrumStateType;
SpektrumStateType PrimarySpektrumState = {1,0,0,0,0,0,0,0,0};
#ifdef RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT
+#warning "Using secondary spektrum receiver."
SpektrumStateType SecondarySpektrumState = {1,0,0,0,0,0,0,0,0};
+#else
+#warning "NOT using secondary spektrum receiver."
#endif
int16_t SpektrumBuf[SPEKTRUM_CHANNELS_PER_FRAME*MAX_SPEKTRUM_FRAMES];
@@ -546,6 +551,9 @@ void SpektrumUartInit(void) {
USART_Init(PrimaryUart(_reg), &usart);
/* Enable Primary UART Receive interrupts */
USART_ITConfig(PrimaryUart(_reg), USART_IT_RXNE, ENABLE);
+
+ /* required to get the correct baudrate on lisa m */
+ pprz_usart_set_baudrate(PrimaryUart(_reg), B115200);
/* Enable the Primary UART */
USART_Cmd(PrimaryUart(_reg), ENABLE);
@@ -577,6 +585,9 @@ void SpektrumUartInit(void) {
USART_Init(SecondaryUart(_reg), &usart);
/* Enable Secondary UART Receive interrupts */
USART_ITConfig(SecondaryUart(_reg), USART_IT_RXNE, ENABLE);
+
+ /* required to get the correct baudrate on lisa m */
+ pprz_usart_set_baudrate(SecondaryUart(_reg), B115200);
/* Enable the Primary UART */
USART_Cmd(SecondaryUart(_reg), ENABLE);
#endif
@@ -692,7 +703,7 @@ void radio_control_spektrum_try_bind(void) {
#endif
/* We have no idea how long the window for allowing binding after
- power up is .This works for the moment but will need revisiting */
+ power up is. This works for the moment but will need revisiting */
DelayMs(61);
for (int i = 0; i < MASTER_RECEIVER_PULSES ; i++)
diff --git a/sw/airborne/arch/stm32/subsystems/settings_arch.c b/sw/airborne/arch/stm32/subsystems/settings_arch.c
new file mode 100644
index 0000000000..ee5f21a598
--- /dev/null
+++ b/sw/airborne/arch/stm32/subsystems/settings_arch.c
@@ -0,0 +1,270 @@
+/*
+ * Paparazzi persistent settings low level flash routines stm32
+ *
+ * Copyright (C) 2011 Martin Mueller
+ *
+ * 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.
+ *
+ */
+
+/*
+ flash data is located in the last page/sector of flash
+
+ data flash_addr
+ data_size flash_end - FSIZ
+ checksum flash_end - FCHK
+
+ STM32: minimum write size 2 bytes, endurance 10k cycles,
+ max sector erase time 40ms, max prog time 70us per 2 bytes
+*/
+
+#include "subsystems/settings.h"
+
+#include
+#include
+#include
+
+struct FlashInfo {
+ uint32_t addr;
+ uint32_t total_size;
+ uint32_t page_nr;
+ uint32_t page_size;
+};
+
+
+static uint32_t pflash_checksum(uint32_t ptr, uint32_t size);
+static int32_t flash_detect(struct FlashInfo* flash);
+static int32_t pflash_program_bytes(struct FlashInfo* flash,
+ uint32_t src,
+ uint32_t size,
+ uint32_t chksum);
+
+#define FLASH_SIZE_ MMIO16(0x1FFFF7E0)
+
+#define FLASH_BEGIN 0x08000000
+#define FSIZ 8
+#define FCHK 4
+
+
+static uint32_t pflash_checksum(uint32_t ptr, uint32_t size) {
+ uint32_t i;
+
+ /* reset crc */
+ CRC_CR = CRC_CR_RESET;
+
+ if (ptr % 4) {
+ /* calc in 8bit chunks */
+ for (i=0; i<(size & ~3); i+=4) {
+ CRC_DR = (*(uint8_t*) (ptr+i)) |
+ (*(uint8_t*) (ptr+i+1)) << 8 |
+ (*(uint8_t*) (ptr+i+2)) << 16 |
+ (*(uint8_t*) (ptr+i+3)) << 24;
+ }
+ } else {
+ /* calc in 32bit */
+ for (i=0; i<(size & ~3); i+=4) {
+ CRC_DR = *(uint32_t*) (ptr+i);
+ }
+ }
+
+ /* remaining bytes */
+ switch (size % 4) {
+ case 1:
+ CRC_DR = *(uint8_t*) (ptr+i);
+ break;
+ case 2:
+ CRC_DR = (*(uint8_t*) (ptr+i)) |
+ (*(uint8_t*) (ptr+i+1)) << 8;
+ break;
+ case 3:
+ CRC_DR = (*(uint8_t*) (ptr+i)) |
+ (*(uint8_t*) (ptr+i+1)) << 8 |
+ (*(uint8_t*) (ptr+i+2)) << 16;
+ break;
+ default:
+ break;
+ }
+
+ return CRC_DR;
+}
+
+static int32_t flash_detect(struct FlashInfo* flash) {
+ uint32_t device_id;
+
+ flash->total_size = FLASH_SIZE_ * 0x400;
+
+#if 1
+ /* FIXME This will not work for connectivity line (needs ID, see below), but
+ device ID is only readable when freshly loaded through JTAG?! */
+
+ switch (flash->total_size) {
+ /* low density */
+ case 0x00004000: /* 16 kBytes */
+ case 0x00008000: /* 32 kBytes */
+ /* medium density, e.g. STM32F103RBT6 (Olimex STM32-H103) */
+ case 0x00010000: /* 64 kBytes */
+ case 0x00200000: /* 128 kBytes */
+ {
+ flash->page_size = 0x400;
+ break;
+ }
+ /* high density, e.g. STM32F103RE (Joby Lisa/M, Lisa/L) */
+ case 0x00040000: /* 256 kBytes */
+ case 0x00080000: /* 512 kBytes */
+ /* XL density */
+ case 0x000C0000: /* 768 kBytes */
+ case 0x00100000: /* 1 MByte */
+ {
+ flash->page_size = 0x800;
+ break;
+ }
+ default: {return -1;}
+ }
+
+#else /* this is the correct way of detecting page sizes */
+
+ /* read device id */
+ device_id = DBGMCU_IDCODE & DBGMCU_IDCODE_DEV_ID_MASK;
+
+ switch (device_id) {
+ /* low density */
+ case 0x412:
+ /* medium density, e.g. STM32F103RB (Olimex STM32-H103) */
+ case 0x410:
+ {
+ flash->page_size = 0x400;
+ break;
+ }
+ /* high density, e.g. STM32F103RE (Joby Lisa/L) */
+ case 0x414:
+ /* XL density */
+ case 0x430:
+ /* connectivity line */
+ case 0x418:
+ {
+ flash->page_size = 0x800;
+ break;
+ }
+ default: return -1;
+ }
+
+ switch (flash->total_size) {
+ case 0x00004000: /* 16 kBytes */
+ case 0x00008000: /* 32 kBytes */
+ case 0x00010000: /* 64 kBytes */
+ case 0x00200000: /* 128 kBytes */
+ case 0x00040000: /* 256 kBytes */
+ case 0x00080000: /* 512 kBytes */
+ case 0x000C0000: /* 768 kBytes */
+ case 0x00100000: /* 1 MByte */
+ break;
+ default: return -1;
+ }
+#endif
+
+ flash->page_nr = (flash->total_size / flash->page_size) - 1;
+ flash->addr = FLASH_BEGIN + flash->page_nr * flash->page_size;
+
+ return 0;
+}
+
+// (gdb) p *flash
+// $1 = {addr = 134739968, total_size = 524288, page_nr = 255, page_size = 2048}
+// 0x807F800 0x80000
+
+static int32_t pflash_program_bytes(struct FlashInfo* flash,
+ uint32_t src,
+ uint32_t size,
+ uint32_t chksum) {
+ uint32_t i;
+
+ /* erase */
+ flash_unlock();
+ flash_erase_page(flash->addr);
+ flash_lock();
+
+ /* verify erase */
+ for (i=0; ipage_size; i+=4) {
+ if ((*(uint32_t*) (flash->addr + i)) != 0xFFFFFFFF) return -1;
+ }
+
+ flash_unlock();
+ /* write full 16 bit words */
+ for (i=0; i<(size & ~1); i+=2) {
+ flash_program_half_word(flash->addr+i,
+ (uint16_t)(*(uint8_t*)(src+i) | (*(uint8_t*)(src+i+1)) << 8));
+ }
+ /* fill bytes with a zero */
+ if (size & 1) {
+ flash_program_half_word(flash->addr+i, (uint16_t)(*(uint8_t*)(src+i)));
+ }
+ /* write size */
+ flash_program_half_word(flash->addr+flash->page_size-FSIZ,
+ (uint16_t)(size & 0xFFFF));
+ flash_program_half_word(flash->addr+flash->page_size-FSIZ+2,
+ (uint16_t)((size >> 16) & 0xFFFF));
+ /* write checksum */
+ flash_program_half_word(flash->addr+flash->page_size-FCHK,
+ (uint16_t)(chksum & 0xFFFF));
+ flash_program_half_word(flash->addr+flash->page_size-FCHK+2,
+ (uint16_t)((chksum >> 16) & 0xFFFF));
+ flash_lock();
+
+ /* verify data */
+ for (i=0; iaddr+i)) != (*(uint8_t*) (src+i))) return -2;
+ }
+ if (*(uint32_t*) (flash->addr+flash->page_size-FSIZ) != size) return -3;
+ if (*(uint32_t*) (flash->addr+flash->page_size-FCHK) != chksum) return -4;
+
+ return 0;
+}
+
+int32_t persistent_write(uint32_t ptr, uint32_t size) {
+ struct FlashInfo flash_info;
+ if (flash_detect(&flash_info)) return -1;
+ if ((size > flash_info.page_size-FSIZ) || (size == 0)) return -2;
+
+ return pflash_program_bytes(&flash_info,
+ ptr,
+ size,
+ pflash_checksum(ptr, size));
+}
+
+int32_t persistent_read(uint32_t ptr, uint32_t size) {
+ struct FlashInfo flash;
+ uint32_t i;
+
+ /* check parameters */
+ if (flash_detect(&flash)) return -1;
+ if ((size > flash.page_size-FSIZ) || (size == 0)) return -2;
+
+ /* check consistency */
+ if (size != *(uint32_t*)(flash.addr+flash.page_size-FSIZ)) return -3;
+ if (pflash_checksum(flash.addr, size) !=
+ *(uint32_t*)(flash.addr+flash.page_size-FCHK))
+ return -4;
+
+ /* copy data */
+ for (i=0; i
+ * Copyright (C) 2011 Martin Mueller
*
- * This file is part of paparazzi.
+ * This file is part of Paparazzi.
*
- * paparazzi is free software; you can redistribute it and/or modify
+ * 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,
+ * 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
+ * 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 "mcu_periph/uart.h"
-//#include "mcu_periph/i2c.h"
-void uart0_init( void ) {}
-void uart1_init( void ) {}
+#ifndef STM32_SUBSYSTEMS_SETTINGS_H
+#define STM32_SUBSYSTEMS_SETTINGS_H
-//void i2c0_hw_init( void ) {}
-//void i2c1_hw_init( void ) {}
+
+
+#endif /* STM32_SUBSYSTEMS_SETTINGS_H */
diff --git a/sw/airborne/boards/booz/baro_board.c b/sw/airborne/boards/booz/baro_board.c
index aa43c684a5..482aa7944b 100644
--- a/sw/airborne/boards/booz/baro_board.c
+++ b/sw/airborne/boards/booz/baro_board.c
@@ -31,20 +31,20 @@
#define BOOZ_ANALOG_BARO_THRESHOLD 850
#endif
-// pressure on AD0.1 on P0.28
-// offset on DAC on P0.25
-
struct Baro baro;
struct BaroBoard baro_board;
+
void baro_init( void ) {
+
+ adc_buf_channel(ADC_CHANNEL_BARO, &baro_board.buf, DEFAULT_AV_NB_SAMPLE);
baro.status = BS_UNINITIALIZED;
baro.absolute = 0;
baro.differential = 0; /* not handled on this board */
baro_board.offset = 1023;
- Booz2AnalogSetDAC(baro_board.offset);
+ DACSet(baro_board.offset);
baro_board.value_filtered = 0;
baro_board.data_available = FALSE;
@@ -53,7 +53,16 @@ void baro_init( void ) {
#endif
}
-void baro_periodic(void) {}
+void baro_periodic(void) {
+
+ baro.absolute = baro_board.buf.sum/baro_board.buf.av_nb_sample;
+ baro_board.value_filtered = (3*baro_board.value_filtered + baro.absolute)/4;
+ if (baro.status == BS_UNINITIALIZED) {
+ RunOnceEvery(10, { baro_board_calibrate();});
+ }
+ /* else */
+ baro_board.data_available = TRUE;
+}
/* decrement offset until adc reading is over a threshold */
void baro_board_calibrate(void) {
@@ -62,7 +71,7 @@ void baro_board_calibrate(void) {
baro_board.offset -= 15;
else
baro_board.offset--;
- Booz2AnalogSetDAC(baro_board.offset);
+ DACSet(baro_board.offset);
#ifdef ROTORCRAFT_BARO_LED
LED_TOGGLE(ROTORCRAFT_BARO_LED);
#endif
diff --git a/sw/airborne/boards/booz/baro_board.h b/sw/airborne/boards/booz/baro_board.h
index f1fae16a48..902aef2db1 100644
--- a/sw/airborne/boards/booz/baro_board.h
+++ b/sw/airborne/boards/booz/baro_board.h
@@ -4,14 +4,15 @@
#include "std.h"
#include "subsystems/sensors/baro.h"
-#include "booz/booz2_analog.h"
+#include "mcu_periph/adc.h"
+#include "mcu_periph/dac.h"
-/* we don't need that on this board */
struct BaroBoard {
uint16_t offset;
uint16_t value_filtered;
bool_t data_available;
+ struct adc_buf buf;
};
extern struct BaroBoard baro_board;
@@ -27,20 +28,8 @@ extern void baro_board_calibrate(void);
static inline void baro_board_SetOffset(uint16_t _o) {
baro_board.offset = _o;
- Booz2AnalogSetDAC(_o);
+ DACSet(_o);
}
-static inline void BoozBaroISRHandler(uint16_t _val) {
- baro.absolute = _val;
- baro_board.value_filtered = (3*baro_board.value_filtered + baro.absolute)/4;
- if (baro.status == BS_UNINITIALIZED) {
- RunOnceEvery(10, { baro_board_calibrate();});
- }
- /* else */
- baro_board.data_available = TRUE;
-}
-
-
-
#endif /* BOARDS_BOOZ_BARO_H */
diff --git a/sw/airborne/boards/booz_1.0.h b/sw/airborne/boards/booz_1.0.h
index 7634648333..db7e8069b6 100644
--- a/sw/airborne/boards/booz_1.0.h
+++ b/sw/airborne/boards/booz_1.0.h
@@ -54,31 +54,77 @@
/* ADC */
-/* pressure : P0.10 AD1.2 */
-#define ANALOG_BARO_PINSEL PINSEL0
-#define ANALOG_BARO_PINSEL_VAL 0x03
-#define ANALOG_BARO_PINSEL_BIT 20
-#define ANALOG_BARO_ADC 1
+/* select P0.13 (ADC_SPARE) as AD1.4 for ADC_0 */
+#define ADC_0 AdcBank1(4)
+#ifdef USE_ADC_0
+#ifndef USE_AD1
+#define USE_AD1
+#endif
+#define USE_AD1_4
+#endif
+
+/* select P0.4 (SCK_0) as AD0.6 for ADC_1 */
+#define ADC_1 AdcBank0(6)
+#ifdef USE_ADC_1
+#ifndef USE_AD0
+#define USE_AD0
+#endif
+#define USE_AD0_6
+#endif
+
+/* select P0.5 (MISO_0) as AD0.7 for ADC_2 */
+#define ADC_2 AdcBank0(7)
+#ifdef USE_ADC_2
+#ifndef USE_AD0
+#define USE_AD0
+#endif
+#define USE_AD0_7
+#endif
+
+/* select P0.6 (MOSI_0) as AD1.0 for ADC_3 */
+#define ADC_3 AdcBank1(0)
+#ifdef USE_ADC_3
+#ifndef USE_AD1
+#define USE_AD1
+#endif
+#define USE_AD1_0
+#endif
+
+/* battery */
+#define ADC_CHANNEL_VSUPPLY AdcBank0(2)
+#ifndef USE_AD0
+#define USE_AD0
+#endif
+#define USE_AD0_2
+
+#define DefaultVoltageOfAdc(adc) (0.0183*adc)
+
+/* baro */
+#define ADC_CHANNEL_BARO AdcBank1(2)
+#ifndef USE_AD1
+#define USE_AD1
+#endif
+#define USE_AD1_2
-/* MS2001 on SSP, IMU connector */
-#define MS2001_SS_PIN 28
-#define MS2001_SS_IODIR IO1DIR
-#define MS2001_SS_IOSET IO1SET
-#define MS2001_SS_IOCLR IO1CLR
+/* MS2100 on SSP, IMU connector */
+#define MS2100_SS_PIN 28
+#define MS2100_SS_IODIR IO1DIR
+#define MS2100_SS_IOSET IO1SET
+#define MS2100_SS_IOCLR IO1CLR
-#define MS2001_RESET_PIN 19
-#define MS2001_RESET_IODIR IO1DIR
-#define MS2001_RESET_IOSET IO1SET
-#define MS2001_RESET_IOCLR IO1CLR
+#define MS2100_RESET_PIN 19
+#define MS2100_RESET_IODIR IO1DIR
+#define MS2100_RESET_IOSET IO1SET
+#define MS2100_RESET_IOCLR IO1CLR
-#define MS2001_DRDY_PIN 30
-#define MS2001_DRDY_PINSEL PINSEL1
-#define MS2001_DRDY_PINSEL_BIT 28
-#define MS2001_DRDY_PINSEL_VAL 2
-#define MS2001_DRDY_EINT 3
-#define MS2001_DRDY_VIC_IT VIC_EINT3
+#define MS2100_DRDY_PIN 30
+#define MS2100_DRDY_PINSEL PINSEL1
+#define MS2100_DRDY_PINSEL_BIT 28
+#define MS2100_DRDY_PINSEL_VAL 2
+#define MS2100_DRDY_EINT 3
+#define MS2100_DRDY_VIC_IT VIC_EINT3
/* PWM5 on CAM connector */
/* P0.21 */
diff --git a/sw/airborne/boards/lisa_l_1.0.h b/sw/airborne/boards/lisa_l_1.0.h
index 8724588595..0923a56a40 100644
--- a/sw/airborne/boards/lisa_l_1.0.h
+++ b/sw/airborne/boards/lisa_l_1.0.h
@@ -1,11 +1,14 @@
-#ifndef CONFIG_LISA_V1_0_H
-#define CONFIG_LISA_V1_0_H
+#ifndef CONFIG_LISA_L_1_0_H
+#define CONFIG_LISA_L_1_0_H
+#define BOARD_LISA_L
#define AHB_CLK 72000000
/* Lisa uses an external clock instead of a crystal */
#define HSE_TYPE_EXT_CLK
+#define STM32_RCC_MODE RCC_HSE_Bypass
+#define STM32_PLL_MULT RCC_PLLMul_9
/* Onboard LEDs */
#define LED_1_BANK
@@ -20,9 +23,17 @@
#define IMU_ACC_DRDY_GPIO_PORTSOURCE GPIO_PortSourceGPIOD
+/* PA0 - ADC0 */
#define ADC_CHANNEL_VSUPPLY 2
#define DefaultVoltageOfAdc(adc) (0.0059*adc)
+/* Onboard ADCs */
+#define BOARD_ADC_CHANNEL_1 ADC_Channel_8
+#define BOARD_ADC_CHANNEL_2 ADC_Channel_9
+// FIXME - removed for now and used for battery monitoring
+//#define BOARD_ADC_CHANNEL_3 ADC_Channel_13
+#define BOARD_ADC_CHANNEL_3 ADC_Channel_0
+#define BOARD_ADC_CHANNEL_4 ADC_Channel_15
#define BOARD_HAS_BARO
-#endif /* CONFIG_LISA_V1_0_H */
+#endif /* CONFIG_LISA_L_1_0_H */
diff --git a/sw/airborne/boards/lisa_m/baro_board.c b/sw/airborne/boards/lisa_m/baro_board.c
new file mode 100644
index 0000000000..8e686c004e
--- /dev/null
+++ b/sw/airborne/boards/lisa_m/baro_board.c
@@ -0,0 +1,150 @@
+
+#include "subsystems/sensors/baro.h"
+#include
+
+struct Baro baro;
+struct BaroBoard baro_board;
+struct i2c_transaction baro_trans;
+struct bmp085_baro_calibration calibration;
+
+#define BMP085_SAMPLE_PERIOD_MS (3 + (2 << BMP085_OSS) * 3)
+#define BMP085_SAMPLE_PERIOD (BMP075_SAMPLE_PERIOD_MS >> 1)
+
+// FIXME: BARO DRDY connected to PB0 for lisa/m
+
+static inline void bmp085_write_reg(uint8_t addr, uint8_t value)
+{
+ baro_trans.type = I2CTransTx;
+ baro_trans.slave_addr = BMP085_ADDR;
+ baro_trans.len_w = 2;
+ baro_trans.buf[0] = addr;
+ baro_trans.buf[1] = value;
+ i2c_submit(&i2c2, &baro_trans);
+ while (baro_trans.status == I2CTransPending || baro_trans.status == I2CTransRunning);
+}
+
+static inline void bmp085_read_reg16(uint8_t addr)
+{
+ baro_trans.type = I2CTransTxRx;
+ baro_trans.slave_addr = BMP085_ADDR;
+ baro_trans.len_w = 1;
+ baro_trans.len_r = 2;
+ baro_trans.buf[0] = addr;
+ i2c_submit(&i2c2, &baro_trans);
+}
+
+static inline int16_t bmp085_read_reg16_blocking(uint8_t addr)
+{
+ bmp085_read_reg16(addr);
+
+ while (baro_trans.status == I2CTransPending || baro_trans.status == I2CTransRunning);
+
+ return ((baro_trans.buf[0] << 8) | baro_trans.buf[1]);
+}
+
+static inline void bmp085_read_reg24(uint8_t addr)
+{
+ baro_trans.type = I2CTransTxRx;
+ baro_trans.slave_addr = BMP085_ADDR;
+ baro_trans.len_w = 1;
+ baro_trans.len_r = 3;
+ baro_trans.buf[0] = addr;
+ i2c_submit(&i2c2, &baro_trans);
+}
+
+static void bmp085_baro_read_calibration(void)
+{
+ calibration.ac1 = bmp085_read_reg16_blocking(0xAA); // AC1
+ calibration.ac2 = bmp085_read_reg16_blocking(0xAC); // AC2
+ calibration.ac3 = bmp085_read_reg16_blocking(0xAE); // AC3
+ calibration.ac4 = bmp085_read_reg16_blocking(0xB0); // AC4
+ calibration.ac5 = bmp085_read_reg16_blocking(0xB2); // AC5
+ calibration.ac6 = bmp085_read_reg16_blocking(0xB4); // AC6
+ calibration.b1 = bmp085_read_reg16_blocking(0xB6); // B1
+ calibration.b2 = bmp085_read_reg16_blocking(0xB8); // B2
+ calibration.mb = bmp085_read_reg16_blocking(0xBA); // MB
+ calibration.mc = bmp085_read_reg16_blocking(0xBC); // MC
+ calibration.md = bmp085_read_reg16_blocking(0xBE); // MD
+}
+
+void baro_init(void) {
+ baro.status = BS_UNINITIALIZED;
+ baro.absolute = 0;
+ baro.differential = 0;
+ baro_board.status = LBS_UNINITIALIZED;
+ bmp085_baro_read_calibration();
+
+ /* STM32 specific (maybe this is a LISA/M specific driver anyway?) */
+ GPIO_InitTypeDef GPIO_InitStructure;
+ GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ GPIO_Init(GPIOB, &GPIO_InitStructure);
+}
+
+static inline int baro_eoc(void)
+{
+ return GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_0);
+}
+
+static inline void bmp085_request_pressure(void)
+{
+ bmp085_write_reg(0xF4, 0x34 + (BMP085_OSS << 6));
+}
+
+static inline void bmp085_request_temp(void)
+{
+ bmp085_write_reg(0xF4, 0x2E);
+}
+
+static inline void bmp085_read_pressure(void)
+{
+ bmp085_read_reg24(0xF6);
+}
+
+static inline void bmp085_read_temp(void)
+{
+ bmp085_read_reg16(0xF6);
+}
+
+void baro_periodic(void) {
+ // check that nothing is in progress
+ if (baro_trans.status == I2CTransPending) return;
+ if (baro_trans.status == I2CTransRunning) return;
+ if (!i2c_idle(&i2c2)) return;
+
+ switch (baro_board.status) {
+ case LBS_UNINITIALIZED:
+ baro_board_send_reset();
+ baro_board.status = LBS_REQUEST;
+ baro.status = BS_RUNNING;
+ break;
+ case LBS_REQUEST:
+ bmp085_request_pressure();
+ baro_board.status = LBS_READ;
+ break;
+ case LBS_READ:
+ if (baro_eoc()) {
+ bmp085_read_pressure();
+ baro_board.status = LBS_READING;
+ }
+ break;
+ case LBS_REQUEST_TEMP:
+ bmp085_request_temp();
+ baro_board.status = LBS_READ_TEMP;
+ break;
+ case LBS_READ_TEMP:
+ if (baro_eoc()) {
+ bmp085_read_temp();
+ baro_board.status = LBS_READING_TEMP;
+ }
+ break;
+ default:
+ break;
+ }
+
+}
+
+void baro_board_send_reset(void) {
+ // This is a NOP at the moment
+}
diff --git a/sw/airborne/boards/lisa_m/baro_board.h b/sw/airborne/boards/lisa_m/baro_board.h
new file mode 100644
index 0000000000..cbe3454983
--- /dev/null
+++ b/sw/airborne/boards/lisa_m/baro_board.h
@@ -0,0 +1,106 @@
+
+/*
+ * board specific fonctions for the lisa_m board
+ *
+ */
+
+#ifndef BOARDS_LISA_M_BARO_H
+#define BOARDS_LISA_M_BARO_H
+
+#include "std.h"
+#include "mcu_periph/i2c.h"
+
+// absolute addr
+#define BMP085_ADDR 0xEE
+// Over sample setting (0-3)
+#define BMP085_OSS 3
+
+enum LisaBaroStatus {
+ LBS_UNINITIALIZED,
+ LBS_REQUEST,
+ LBS_READING,
+ LBS_READ,
+ LBS_REQUEST_TEMP,
+ LBS_READING_TEMP,
+ LBS_READ_TEMP,
+};
+
+struct BaroBoard {
+ enum LisaBaroStatus status;
+};
+
+struct bmp085_baro_calibration {
+ // These values come from EEPROM on sensor
+ int16_t ac1;
+ int16_t ac2;
+ int16_t ac3;
+ uint16_t ac4;
+ uint16_t ac5;
+ uint16_t ac6;
+ int16_t b1;
+ int16_t b2;
+ int16_t mb;
+ int16_t mc;
+ int16_t md;
+
+ // These values are calculated
+ int32_t b5;
+};
+
+extern struct BaroBoard baro_board;
+extern struct i2c_transaction baro_trans;
+extern struct bmp085_baro_calibration calibration;
+
+extern void baro_board_send_reset(void);
+extern void baro_board_send_config(void);
+
+// Apply temp calibration and sensor calibration to raw measurement to get Pa (from BMP085 datasheet)
+static inline int32_t baro_apply_calibration(int32_t raw)
+{
+ int32_t b6 = calibration.b5 - 4000;
+ int x1 = (calibration.b2 * (b6 * b6 >> 12)) >> 11;
+ int x2 = calibration.ac2 * b6 >> 11;
+ int32_t x3 = x1 + x2;
+ int32_t b3 = (((calibration.ac1 * 4 + x3) << BMP085_OSS) + 2)/4;
+ x1 = calibration.ac3 * b6 >> 13;
+ x2 = (calibration.b1 * (b6 * b6 >> 12)) >> 16;
+ x3 = ((x1 + x2) + 2) >> 2;
+ uint32_t b4 = (calibration.ac4 * (uint32_t) (x3 + 32768)) >> 15;
+ uint32_t b7 = (raw - b3) * (50000 >> BMP085_OSS);
+ int32_t p = b7 < 0x80000000 ? (b7 * 2) / b4 : (b7 / b4) * 2;
+ x1 = (p >> 8) * (p >> 8);
+ x1 = (x1 * 3038) >> 16;
+ x2 = (-7357 * p) >> 16;
+ return p + ((x1 + x2 + 3791) >> 4);
+}
+
+static inline void baro_event(void (*b_abs_handler)(void), void (*b_diff_handler)(void))
+{
+ if (baro_board.status == LBS_READING &&
+ baro_trans.status != I2CTransPending && baro_trans.status != I2CTransRunning) {
+ baro_board.status = LBS_REQUEST_TEMP;
+ if (baro_trans.status == I2CTransSuccess) {
+ int32_t tmp = (baro_trans.buf[0]<<16) | (baro_trans.buf[1] << 8) | baro_trans.buf[2];
+ tmp = tmp >> (8 - BMP085_OSS);
+ baro.absolute = baro_apply_calibration(tmp);
+ b_abs_handler();
+ }
+ }
+ if (baro_board.status == LBS_READING_TEMP &&
+ baro_trans.status != I2CTransPending && baro_trans.status != I2CTransRunning) {
+ baro_board.status = LBS_REQUEST;
+ if (baro_trans.status == I2CTransSuccess) {
+ // abuse differential to store temp in 0.1C for now
+ int32_t tmp = (baro_trans.buf[0] << 8) | baro_trans.buf[1];
+ int32_t x1 = ((tmp - calibration.ac6) * calibration.ac5) >> 15;
+ int32_t x2 = (calibration.mc << 11) / (x1 + calibration.md);
+ calibration.b5 = x1 + x2;
+ baro.differential = (calibration.b5 + 8) >> 4;
+ b_diff_handler();
+ }
+ }
+}
+
+#define BaroEvent(_b_abs_handler, _b_diff_handler) baro_event(_b_abs_handler,_b_diff_handler)
+
+#endif /* BOARDS_LISA_M_BARO_H */
diff --git a/sw/airborne/boards/lisa_m/test_baro.c b/sw/airborne/boards/lisa_m/test_baro.c
new file mode 100644
index 0000000000..8a24041f53
--- /dev/null
+++ b/sw/airborne/boards/lisa_m/test_baro.c
@@ -0,0 +1,108 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2009 Antoine Drouin
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/*
+ *
+ * test baro using interrupts
+ *
+ */
+
+#include BOARD_CONFIG
+
+#include "mcu.h"
+#include "sys_time.h"
+#include "mcu_periph/uart.h"
+
+#include "downlink.h"
+
+#include "subsystems/sensors/baro.h"
+//#include "my_debug_servo.h"
+
+static inline void main_init( void );
+static inline void main_periodic_task( void );
+static inline void main_event_task( void );
+
+static inline void main_on_baro_diff(void);
+static inline void main_on_baro_abs(void);
+
+
+int main(void) {
+ main_init();
+
+ while(1) {
+ if (sys_time_periodic())
+ main_periodic_task();
+ main_event_task();
+ }
+
+ return 0;
+}
+
+static inline void main_init( void ) {
+ mcu_init();
+ sys_time_init();
+ baro_init();
+
+ // DEBUG_SERVO1_INIT();
+ // DEBUG_SERVO2_INIT();
+
+
+}
+
+
+
+static inline void main_periodic_task( void ) {
+
+ RunOnceEvery(2, {baro_periodic();});
+ LED_PERIODIC();
+ RunOnceEvery(256, {DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);});
+ RunOnceEvery(256,
+ {
+ DOWNLINK_SEND_I2C_ERRORS(DefaultChannel,
+ &i2c2_errors.ack_fail_cnt,
+ &i2c2_errors.miss_start_stop_cnt,
+ &i2c2_errors.arb_lost_cnt,
+ &i2c2_errors.over_under_cnt,
+ &i2c2_errors.pec_recep_cnt,
+ &i2c2_errors.timeout_tlow_cnt,
+ &i2c2_errors.smbus_alert_cnt,
+ &i2c2_errors.unexpected_event_cnt,
+ &i2c2_errors.last_unexpected_event);
+ });
+}
+
+
+
+static inline void main_event_task( void ) {
+ BaroEvent(main_on_baro_abs, main_on_baro_diff);
+}
+
+
+
+static inline void main_on_baro_diff(void) {
+
+}
+
+static inline void main_on_baro_abs(void) {
+ RunOnceEvery(5,{DOWNLINK_SEND_BARO_RAW(DefaultChannel, &baro.absolute, &baro.differential);});
+}
diff --git a/sw/airborne/boards/lisa_m_1.0.h b/sw/airborne/boards/lisa_m_1.0.h
index e87613b155..fd3ddee6bb 100644
--- a/sw/airborne/boards/lisa_m_1.0.h
+++ b/sw/airborne/boards/lisa_m_1.0.h
@@ -1,27 +1,65 @@
#ifndef CONFIG_LISA_M_1_0_H
#define CONFIG_LISA_M_1_0_H
+#define BOARD_LISA_M
#define AHB_CLK 72000000
/* Onboard LEDs */
#define LED_1_BANK
#define LED_1_GPIO GPIOB
-#define LED_1_GPIO_CLK RCC_APB2Periph_GPIOB
+#define LED_1_GPIO_CLK RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO
#define LED_1_GPIO_PIN GPIO_Pin_4
+#define LED_1_AFIO_REMAP GPIO_PinRemapConfig(GPIO_Remap_SWJ_NoJTRST, ENABLE)
#define LED_2_BANK
#define LED_2_GPIO GPIOC
#define LED_2_GPIO_CLK RCC_APB2Periph_GPIOC
#define LED_2_GPIO_PIN GPIO_Pin_13
+#define LED_2_AFIO_REMAP ((void)0)
+
/* configuration for aspirin - and more generaly IMUs */
#define IMU_ACC_DRDY_RCC_GPIO RCC_APB2Periph_GPIOB
#define IMU_ACC_DRDY_GPIO GPIOB
#define IMU_ACC_DRDY_GPIO_PORTSOURCE GPIO_PortSourceGPIOB
-#define ADC_CHANNEL_VSUPPLY 4
-#define DefaultVoltageOfAdc(adc) (0.01787109375*adc)
+#define ADC_CHANNEL_VSUPPLY 2
+#define DefaultVoltageOfAdc(adc) (0.00485*adc)
+
+/* Onboard ADCs */
+/*
+ ADC1 PC3/ADC13
+ ADC2 PA0/ADC0
+ ADC3 PC0/ADC10
+ ADC4 PC1/ADC11
+ ADC5 PC5/ADC15
+ ADC6 PA1/ADC1
+ ADC7 PC2/ADC12
+ BATT PC4/ADC14
+*/
+#define BOARD_ADC_CHANNEL_1 ADC_Channel_13
+#define BOARD_ADC_CHANNEL_2 ADC_Channel_0
+// FIXME - removed for now and used for battery monitoring
+//#define BOARD_ADC_CHANNEL_3 ADC_Channel_10
+#define BOARD_ADC_CHANNEL_3 ADC_Channel_14
+#define BOARD_ADC_CHANNEL_4 ADC_Channel_11
+
+#define BOARD_HAS_BARO
+
+#define USE_OPENCM3
+
+#define HSE_TYPE_EXT_CLK
+#define STM32_RCC_MODE RCC_HSE_ON
+#define STM32_PLL_MULT RCC_PLLMul_6
+
+#define PWM_5AND6_TIMER TIM5
+#define PWM_5AND6_RCC RCC_APB1Periph_TIM5
+#define PWM5_OC 1
+#define PWM6_OC 2
+#define PWM_5AND6_GPIO GPIOA
+#define PWM5_Pin GPIO_Pin_0
+#define PWM6_Pin GPIO_Pin_1
#endif /* CONFIG_LISA_M_1_0_H */
diff --git a/sw/airborne/boards/tiny_sim.h b/sw/airborne/boards/tiny_sim.h
index c3bb4a9eea..ff3845ca86 100644
--- a/sw/airborne/boards/tiny_sim.h
+++ b/sw/airborne/boards/tiny_sim.h
@@ -2,20 +2,20 @@
#define CONFIG_TINY_H
/* Master oscillator freq. */
-#define FOSC (12000000)
+#define FOSC (12000000)
/* PLL multiplier */
-#define PLL_MUL (5)
+#define PLL_MUL (5)
/* CPU clock freq. */
-#define CCLK (FOSC * PLL_MUL)
+#define CCLK (FOSC * PLL_MUL)
/* Peripheral bus speed mask 0x00->4, 0x01-> 1, 0x02 -> 2 */
-#define PBSD_BITS 0x00
+#define PBSD_BITS 0x00
#define PBSD_VAL 4
/* Peripheral bus clock freq. */
-#define PCLK (CCLK / PBSD_VAL)
+#define PCLK (CCLK / PBSD_VAL)
#define LED_1_BANK 1
#define LED_1_PIN 28
@@ -80,7 +80,14 @@
#define USE_AD0_1
#endif
-/* #define ADC_3 AdcBank1(7) Used for VSUPPLY */
+#define ADC_3 AdcBank0(6)
+#ifdef USE_ADC_3
+#ifndef USE_AD0
+#define USE_AD0
+#endif
+#define USE_AD0_6
+#endif
+
#define ADC_4 AdcBank1(3)
#ifdef USE_ADC_4
@@ -114,6 +121,7 @@
#define USE_AD1_2
#endif
+/* #define ADC_3 AdcBank1(7) Used for VSUPPLY */
#define ADC_CHANNEL_VSUPPLY AdcBank1(7)
#ifndef USE_AD1
#define USE_AD1
diff --git a/sw/airborne/booz/arch/lpc21/booz2_analog_hw.c b/sw/airborne/booz/arch/lpc21/booz2_analog_hw.c
deleted file mode 100644
index a991d3e3ed..0000000000
--- a/sw/airborne/booz/arch/lpc21/booz2_analog_hw.c
+++ /dev/null
@@ -1,229 +0,0 @@
-/*
- * $Id$
- *
- * Copyright (C) 2008-2009 Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- */
-
-#include "booz2_analog.h"
-
-/* analog_arch includes baro ??? naaaa we don't want double references */
-#include "subsystems/sensors/baro.h"
-#include "firmwares/rotorcraft/battery.h"
-
-#ifndef USE_EXTRA_ADC
-/* Default mode
- * Bust OFF
- * Only one ADC can be read on each bank
- * Baro and Bat are read on interrupt
- */
-
-#include "armVIC.h"
-#include "sys_time.h"
-
-void ADC0_ISR ( void ) __attribute__((naked));
-void ADC1_ISR ( void ) __attribute__((naked));
-
-void booz2_analog_init_hw( void ) {
-
- /* start ADC0 */
- /* select P0.29 as AD0.2 for bat meas*/
- PINSEL1 |= 0x01 << 26;
- /* sample AD0.2 - PCLK/4 ( 3.75MHz) - ON */
- AD0CR = 1 << 2 | 0x03 << 8 | 1 << 21;
- /* AD0 selected as IRQ */
- VICIntSelect &= ~VIC_BIT(VIC_AD0);
- /* AD0 interrupt enabled */
- VICIntEnable = VIC_BIT(VIC_AD0);
- /* AD0 interrupt as VIC2 */
- _VIC_CNTL(ADC0_VIC_SLOT) = VIC_ENABLE | VIC_AD0;
- _VIC_ADDR(ADC0_VIC_SLOT) = (uint32_t)ADC0_ISR;
- /* start convertion on T0M1 match */
- AD0CR |= 4 << 24;
-
-
- /* clear match 1 */
- T0EMR &= ~TEMR_EM1;
- /* set high on match 1 */
- T0EMR |= TEMR_EMC1_2;
- /* first match in a while */
- T0MR1 = 1024;
-
-
- /* start ADC1 */
- /* select P0.10 as AD1.2 for baro*/
- ANALOG_BARO_PINSEL |= ANALOG_BARO_PINSEL_VAL << ANALOG_BARO_PINSEL_BIT;
- /* sample AD1.2 - PCLK/4 ( 3.75MHz) - ON */
- AD1CR = 1 << 2 | 0x03 << 8 | 1 << 21;
- /* AD0 selected as IRQ */
- VICIntSelect &= ~VIC_BIT(VIC_AD1);
- /* AD0 interrupt enabled */
- VICIntEnable = VIC_BIT(VIC_AD1);
- /* AD0 interrupt as VIC2 */
- _VIC_CNTL(ADC1_VIC_SLOT) = VIC_ENABLE | VIC_AD1;
- _VIC_ADDR(ADC1_VIC_SLOT) = (uint32_t)ADC1_ISR;
- /* start convertion on T0M3 match */
- AD1CR |= 5 << 24;
-
-
- /* clear match 2 */
- T0EMR &= ~TEMR_EM3;
- /* set high on match 2 */
- T0EMR |= TEMR_EMC3_2;
- /* first match in a while */
- T0MR3 = 512;
-
- /* turn on DAC pins */
- PINSEL1 |= 2 << 18;
-}
-
-
-void ADC0_ISR ( void ) {
- ISR_ENTRY();
- uint32_t tmp = AD0GDR;
- uint16_t tmp2 = (uint16_t)(tmp >> 6) & 0x03FF;
- BatteryISRHandler(tmp2);
- /* trigger next convertion */
- T0MR1 += BOOZ2_ANALOG_BATTERY_PERIOD;
- /* lower clock */
- T0EMR &= ~TEMR_EM1;
- VICVectAddr = 0x00000000; // clear this interrupt from the VIC
- ISR_EXIT(); // recover registers and return
-}
-
-void ADC1_ISR ( void ) {
- ISR_ENTRY();
- uint32_t tmp = AD1GDR;
- uint16_t tmp2 = (uint16_t)(tmp >> 6) & 0x03FF;
- BoozBaroISRHandler(tmp2);
- /* trigger next convertion */
- T0MR3 += BOOZ2_ANALOG_BARO_PERIOD;
- /* lower clock */
- T0EMR &= ~TEMR_EM3;
- VICVectAddr = 0x00000000; // clear this interrupt from the VIC
- ISR_EXIT(); // recover registers and return
-}
-
-#else // USE_EXTRA_ADC
-/* Extra ADCs are read
- * Bust ON
- * Baro and Bat values are updated by hand
- * Four ADCs can be configured
- * ADC_1 is available on the cam connector
- */
-
-#include "LPC21xx.h"
-#include "sys_time.h"
-
-uint16_t booz2_adc_1;
-uint16_t booz2_adc_2;
-uint16_t booz2_adc_3;
-uint16_t booz2_adc_4;
-
-void booz2_analog_init_hw( void ) {
-
- /* AD0 */
- /* PCLK/4 ( 3.75MHz) - BURST - ON */
- AD0CR = 0x03 << 8 | 1 << 16 | 1 << 21;
- /* disable global interrupt */
- ClearBit(AD0INTEN,8);
-
- /* AD1 */
- /* PCLK/4 ( 3.75MHz) - BURST - ON */
- AD1CR = 0x03 << 8 | 1 << 16 | 1 << 21;
- /* disable global interrupt */
- ClearBit(AD1INTEN,8);
-
- /* select P0.29 as AD0.2 for bat meas*/
- PINSEL1 |= 0x01 << 26;
- /* sample AD0.2 */
- AD0CR |= 1 << 2;
-
-
- /* select P0.10 as AD1.2 for baro*/
- ANALOG_BARO_PINSEL |= ANALOG_BARO_PINSEL_VAL << ANALOG_BARO_PINSEL_BIT;
- /* sample AD1.2 */
- AD1CR |= 1 << 2;
- /* turn on DAC pins */
- PINSEL1 |= 2 << 18;
-
-#ifdef USE_ADC_1
- /* select P0.13 (ADC_SPARE) as AD1.4 adc 1 */
- PINSEL0 |= 0x03 << 26;
- AD1CR |= 1 << 4;
-#endif
-#ifdef USE_ADC_2
- /* select P0.4 (SCK_0) as AD0.6 adc 2 */
- PINSEL0 |= 0x03 << 8;
- AD0CR |= 1 << 6;
-#endif
-#ifdef USE_ADC_3
- /* select P0.5 (MISO_0) as AD0.7 adc 3 */
- PINSEL0 |= 0x03 << 10;
- AD0CR |= 1 << 7;
-#endif
-#ifdef USE_ADC_4
- /* select P0.6 (MOSI_0) as AD1.0 adc 4 */
- PINSEL0 |= 0x03 << 12;
- AD1CR |= 1 << 0;
-#endif
-
- booz2_adc_1 = 0;
- booz2_adc_2 = 0;
- booz2_adc_3 = 0;
- booz2_adc_4 = 0;
-}
-
-void booz2_analog_baro_read(void) {
- uint32_t tmp = AD1DR2;
- uint16_t tmp2 = (uint16_t)(tmp >> 6) & 0x03FF;
- BoozBaroISRHandler(tmp2);
-}
-
-void booz2_analog_bat_read(void) {
- uint32_t tmp = AD0DR2;
- uint16_t tmp2 = (uint16_t)(tmp >> 6) & 0x03FF;
- BatteryISRHandler(tmp2);
-}
-
-void booz2_analog_extra_adc_read(void) {
- uint32_t tmp,tmp2;
-#ifdef USE_ADC_1
- tmp = AD1DR4;
- tmp2 = (uint16_t)(tmp >> 6) & 0x03FF;
- booz2_adc_1 = tmp2;
-#endif
-#ifdef USE_ADC_2
- tmp = AD0DR6;
- tmp2 = (uint16_t)(tmp >> 6) & 0x03FF;
- booz2_adc_2 = tmp2;
-#endif
-#ifdef USE_ADC_3
- tmp = AD0DR7;
- tmp2 = (uint16_t)(tmp >> 6) & 0x03FF;
- booz2_adc_3 = tmp2;
-#endif
-#ifdef USE_ADC_4
- tmp = AD1DR0;
- tmp2 = (uint16_t)(tmp >> 6) & 0x03FF;
- booz2_adc_4 = tmp2;
-#endif
-}
-
-#endif // USE_EXTRA_ADC
diff --git a/sw/airborne/booz/arch/stm32/booz2_analog_hw.h b/sw/airborne/booz/arch/stm32/booz2_analog_hw.h
deleted file mode 100644
index 9c074731f2..0000000000
--- a/sw/airborne/booz/arch/stm32/booz2_analog_hw.h
+++ /dev/null
@@ -1,33 +0,0 @@
-/*
- * $Id: booz2_analog_hw.h 4172 2009-09-18 11:57:13Z flixr $
- *
- * Copyright (C) 2008-2009 Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- */
-
-#ifndef BOOZ2_ANALOG_HW_H
-#define BOOZ2_ANALOG_HW_H
-
-#include "std.h"
-
-static inline void Booz2AnalogSetDAC(uint16_t x) {}
-
-extern void booz2_analog_init_hw(void);
-
-#endif /* BOOZ2_ANALOG_HW_H */
diff --git a/sw/airborne/booz/booz2_analog.h b/sw/airborne/booz/booz2_analog.h
deleted file mode 100644
index 1c4aa63481..0000000000
--- a/sw/airborne/booz/booz2_analog.h
+++ /dev/null
@@ -1,47 +0,0 @@
-/*
- * $Id$
- *
- * Copyright (C) 2008-2009 Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- */
-
-#ifndef BOOZ2_ANALOG_H
-#define BOOZ2_ANALOG_H
-
-extern void booz2_analog_init( void );
-
-#ifdef USE_EXTRA_ADC
-#include "std.h"
-
-extern uint16_t booz2_adc_1;
-extern uint16_t booz2_adc_2;
-extern uint16_t booz2_adc_3;
-extern uint16_t booz2_adc_4;
-
-extern void booz2_analog_periodic( void );
-
-extern void booz2_analog_baro_read(void);
-extern void booz2_analog_bat_read(void);
-extern void booz2_analog_extra_adc_read(void);
-#endif
-
-
-#include "booz2_analog_hw.h"
-
-#endif /* BOOZ2_ANALOG_H */
diff --git a/sw/airborne/booz/booz_gps.h b/sw/airborne/booz/booz_gps.h
index 7d30611baf..d7d7a7cd8c 100644
--- a/sw/airborne/booz/booz_gps.h
+++ b/sw/airborne/booz/booz_gps.h
@@ -54,14 +54,9 @@ extern struct Booz_gps_state booz_gps_state;
extern void booz_gps_impl_init(void);
-
-/* UBX NAV SOL */
#define BOOZ2_GPS_FIX_NONE 0x00
#define BOOZ2_GPS_FIX_3D 0x03
-
-#include "ubx_protocol.h"
-
#define GpsFixValid() (booz_gps_state.fix == BOOZ2_GPS_FIX_3D)
/*
diff --git a/sw/airborne/booz/gps/booz_gps_skytraq.c b/sw/airborne/booz/gps/booz_gps_skytraq.c
index 6fa2f1ccc8..853fa157c3 100644
--- a/sw/airborne/booz/gps/booz_gps_skytraq.c
+++ b/sw/airborne/booz/gps/booz_gps_skytraq.c
@@ -36,7 +36,7 @@ struct BoozGpsSkytraq booz_gps_skytraq;
#define GOT_CHECKSUM 7
#define GOT_SYNC3 8
-#include "my_debug_servo.h"
+//#include "my_debug_servo.h"
#include "led.h"
void booz_gps_impl_init(void) {
@@ -44,14 +44,14 @@ void booz_gps_impl_init(void) {
booz_gps_skytraq.status = UNINIT;
- DEBUG_SERVO1_INIT();
+ //DEBUG_SERVO1_INIT();
}
void booz_gps_skytraq_read_message(void) {
- DEBUG_S1_ON();
+ //DEBUG_S1_ON();
if (booz_gps_skytraq.msg_id == SKYTRAQ_ID_NAVIGATION_DATA) {
booz_gps_state.ecef_pos.x = SKYTRAQ_NAVIGATION_DATA_ECEFX(booz_gps_skytraq.msg_buf);
@@ -70,7 +70,7 @@ void booz_gps_skytraq_read_message(void) {
booz_gps_state.num_sv = SKYTRAQ_NAVIGATION_DATA_NumSV(booz_gps_skytraq.msg_buf);
booz_gps_state.fix = SKYTRAQ_NAVIGATION_DATA_FixMode(booz_gps_skytraq.msg_buf);
booz_gps_state.tow = SKYTRAQ_NAVIGATION_DATA_TOW(booz_gps_skytraq.msg_buf);
- DEBUG_S2_TOGGLE();
+ //DEBUG_S2_TOGGLE();
#ifdef GPS_LED
if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) {
@@ -82,7 +82,7 @@ void booz_gps_skytraq_read_message(void) {
#endif
}
- DEBUG_S1_OFF();
+ //DEBUG_S1_OFF();
}
void booz_gps_skytraq_parse(uint8_t c) {
diff --git a/sw/airborne/booz/gps/booz_gps_skytraq.h b/sw/airborne/booz/gps/booz_gps_skytraq.h
index f3fe7703f3..27f2345764 100644
--- a/sw/airborne/booz/gps/booz_gps_skytraq.h
+++ b/sw/airborne/booz/gps/booz_gps_skytraq.h
@@ -63,7 +63,7 @@ extern struct BoozGpsSkytraq booz_gps_skytraq;
extern void booz_gps_skytraq_read_message(void);
extern void booz_gps_skytraq_parse(uint8_t c);
-#include "my_debug_servo.h"
+//#include "my_debug_servo.h"
#define BoozGpsEvent(_sol_available_callback) { \
if (GpsBuffer()) { \
diff --git a/sw/airborne/booz/gps/booz_gps_ubx.h b/sw/airborne/booz/gps/booz_gps_ubx.h
index 28d306a0e7..ab2d25f709 100644
--- a/sw/airborne/booz/gps/booz_gps_ubx.h
+++ b/sw/airborne/booz/gps/booz_gps_ubx.h
@@ -1,6 +1,8 @@
#ifndef BOOZ_GPS_UBX_H
#define BOOZ_GPS_UBX_H
+#include "ubx_protocol.h"
+
#define GPS_UBX_MAX_PAYLOAD 255
struct BoosGpsUbx {
bool_t msg_available;
diff --git a/sw/airborne/booz/test/booz2_test_crista.c b/sw/airborne/booz/test/booz2_test_crista.c
index 82f990afe4..1231b7e6ab 100644
--- a/sw/airborne/booz/test/booz2_test_crista.c
+++ b/sw/airborne/booz/test/booz2_test_crista.c
@@ -108,11 +108,11 @@ static inline void on_imu_event(void) {
&imu_accel_unscaled.z);
}
else if (cnt == 7) {
- DOWNLINK_SEND_BOOZ2_GYRO(&imu_gyro.x,
+ DOWNLINK_SEND_IMU_GYRO_SCALED(&imu_gyro.x,
&imu_gyro.y,
&imu_gyro.z);
- DOWNLINK_SEND_BOOZ2_ACCEL(&imu_accel.x,
+ DOWNLINK_SEND_IMU_ACCEL_SCALED(&imu_accel.x,
&imu_accel.y,
&imu_accel.z);
}
diff --git a/sw/airborne/booz/test/booz2_test_radio_control.c b/sw/airborne/booz/test/booz2_test_radio_control.c
index cc0ea59b95..9daf3a9afc 100644
--- a/sw/airborne/booz/test/booz2_test_radio_control.c
+++ b/sw/airborne/booz/test/booz2_test_radio_control.c
@@ -30,7 +30,7 @@
#include "downlink.h"
-#include "booz/booz_radio_control.h"
+#include "subsystems/radio_control.h"
static inline void main_init( void );
static inline void main_periodic_task( void );
@@ -65,7 +65,7 @@ static inline void main_periodic_task( void ) {
DOWNLINK_SEND_TIME(DefaultChannel, &blaaa);
});
- RunOnceEvery(10, {radio_control_periodic();});
+ RunOnceEvery(10, {radio_control_periodic_task();});
int16_t foo = 0;//RC_PPM_SIGNED_TICS_OF_USEC(2050-1500);
RunOnceEvery(10,
diff --git a/sw/airborne/booz/test/booz_test_imu.c b/sw/airborne/booz/test/booz_test_imu.c
index fa4bd2591d..19146b63cb 100644
--- a/sw/airborne/booz/test/booz_test_imu.c
+++ b/sw/airborne/booz/test/booz_test_imu.c
@@ -23,6 +23,9 @@
#include
+#ifdef BOARD_CONFIG
+#include BOARD_CONFIG
+#endif
#include "std.h"
#include "mcu.h"
#include "sys_time.h"
@@ -61,16 +64,20 @@ static inline void main_init( void ) {
sys_time_init();
imu_init();
- DEBUG_SERVO1_INIT();
- DEBUG_SERVO2_INIT();
-
mcu_int_enable();
}
+static inline void led_toggle ( void ) {
+
+#ifdef BOARD_LISA_L
+ LED_TOGGLE(3);
+#endif
+}
+
static inline void main_periodic_task( void ) {
RunOnceEvery(100, {
- LED_TOGGLE(3);
+ led_toggle();
DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
});
#ifdef USE_I2C2
@@ -95,6 +102,7 @@ static inline void main_event_task( void ) {
ImuEvent(on_gyro_accel_event, on_accel_event, on_mag_event);
+
}
static inline void on_accel_event(void) {
@@ -110,7 +118,7 @@ static inline void on_accel_event(void) {
&imu.accel_unscaled.z);
}
else if (cnt == 7) {
- DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel,
+ DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel,
&imu.accel.x,
&imu.accel.y,
&imu.accel.z);
@@ -132,7 +140,7 @@ static inline void on_gyro_accel_event(void) {
&imu.gyro_unscaled.r);
}
else if (cnt == 7) {
- DOWNLINK_SEND_BOOZ2_GYRO(DefaultChannel,
+ DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel,
&imu.gyro.p,
&imu.gyro.q,
&imu.gyro.r);
@@ -147,7 +155,7 @@ static inline void on_mag_event(void) {
if (cnt > 10) cnt = 0;
if (cnt == 0) {
- DOWNLINK_SEND_BOOZ2_MAG(DefaultChannel,
+ DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel,
&imu.mag.x,
&imu.mag.y,
&imu.mag.z);
diff --git a/sw/airborne/csc/mercury_xsens.h b/sw/airborne/csc/mercury_xsens.h
index 9b04d79737..a2f1857a97 100644
--- a/sw/airborne/csc/mercury_xsens.h
+++ b/sw/airborne/csc/mercury_xsens.h
@@ -64,20 +64,20 @@ extern int xsens_setzero;
#include "subsystems/ahrs.h"
-#define PERIODIC_SEND_BOOZ2_GYRO() { \
- DOWNLINK_SEND_BOOZ2_GYRO(&imu.gyro.p, \
+#define PERIODIC_SEND_IMU_GYRO_SCALED() { \
+ DOWNLINK_SEND_IMU_GYRO_SCALED(&imu.gyro.p, \
&imu.gyro.q, \
&imu.gyro.r); \
}
-#define PERIODIC_SEND_BOOZ2_ACCEL() { \
- DOWNLINK_SEND_BOOZ2_ACCEL(&imu.accel.x, \
+#define PERIODIC_SEND_IMU_ACCEL_SCALED() { \
+ DOWNLINK_SEND_IMU_ACCEL_SCALED(&imu.accel.x, \
&imu.accel.y, \
&imu.accel.z); \
}
-#define PERIODIC_SEND_BOOZ2_MAG() { \
- DOWNLINK_SEND_BOOZ2_MAG(&imu.mag.x, \
+#define PERIODIC_SEND_IMU_MAG_SCALED() { \
+ DOWNLINK_SEND_IMU_MAG_SCALED(&imu.mag.x, \
&imu.mag.y, \
&imu.mag.z); \
}
diff --git a/sw/airborne/downlink.c b/sw/airborne/downlink.c
index dbd07749aa..003b816beb 100644
--- a/sw/airborne/downlink.c
+++ b/sw/airborne/downlink.c
@@ -29,6 +29,7 @@
#include "std.h"
+#include "generated/airframe.h"
#ifdef FBW
#ifndef TELEMETRY_MODE_FBW
diff --git a/sw/airborne/downlink.h b/sw/airborne/downlink.h
index 17052669ed..26dcab2988 100644
--- a/sw/airborne/downlink.h
+++ b/sw/airborne/downlink.h
@@ -51,6 +51,9 @@
#include "pprz_transport.h"
#include "modem.h"
#include "xbee.h"
+#ifdef USE_USB_SERIAL
+#include "mcu_periph/usb_serial.h"
+#endif
#endif /** !SITL */
#ifndef DefaultChannel
diff --git a/sw/airborne/firmwares/beth/main_overo.c b/sw/airborne/firmwares/beth/main_overo.c
index 6a52bc090a..b24958321f 100644
--- a/sw/airborne/firmwares/beth/main_overo.c
+++ b/sw/airborne/firmwares/beth/main_overo.c
@@ -159,13 +159,13 @@ static void main_periodic(int my_sig_num) {
RunOnceEvery(10, {DOWNLINK_SEND_IMU_ACCEL_RAW(gcs_com.udp_transport,
//&msg_in.payload.msg_up.accel.x,&msg_in.payload.msg_up.accel.y,&msg_in.payload.msg_up.accel.z
&imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z);})
- RunOnceEvery(50, {DOWNLINK_SEND_BOOZ2_GYRO(gcs_com.udp_transport,
+ RunOnceEvery(50, {DOWNLINK_SEND_IMU_GYRO_SCALED(gcs_com.udp_transport,
//&msg_in.payload.msg_up.gyro.p,&msg_in.payload.msg_up.gyro.q,&msg_in.payload.msg_up.gyro.r)
&imu.gyro.p,&imu.gyro.q,&imu.gyro.r);});
RunOnceEvery(50, {DOWNLINK_SEND_AHRS_EULER(gcs_com.udp_transport,
&estimator.tilt, &estimator.elevation, &estimator.azimuth );});
- RunOnceEvery(50, {DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel,
+ RunOnceEvery(50, {DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel,
//&msg_in.payload.msg_up.accel.x,&msg_in.payload.msg_up.accel.y,&msg_in.payload.msg_up.accel.z
&imu.accel.x,&imu.accel.y,&imu.accel.z);});*/
diff --git a/sw/airborne/firmwares/beth/main_stm32.c b/sw/airborne/firmwares/beth/main_stm32.c
index ee5aca10d2..af09ab6a0a 100644
--- a/sw/airborne/firmwares/beth/main_stm32.c
+++ b/sw/airborne/firmwares/beth/main_stm32.c
@@ -176,12 +176,12 @@ static inline void on_gyro_accel_event(void) {
&imu.accel_unscaled.z);
}
else if (cnt == 7) {
- DOWNLINK_SEND_BOOZ2_GYRO(DefaultChannel,
+ DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel,
&imu.gyro.p,
&imu.gyro.q,
&imu.gyro.r);
- DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel,
+ DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel,
&imu.accel.x,
&imu.accel.y,
&imu.accel.z);
@@ -196,7 +196,7 @@ static inline void on_mag_event(void) {
if (cnt > 1) cnt = 0;
if (cnt%2) {
- DOWNLINK_SEND_BOOZ2_MAG(DefaultChannel,
+ DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel,
&imu.mag.x,
&imu.mag.y,
&imu.mag.z);
diff --git a/sw/airborne/firmwares/fixedwing/datalink.c b/sw/airborne/firmwares/fixedwing/datalink.c
index 89e9c735b7..d94abb5d1d 100644
--- a/sw/airborne/firmwares/fixedwing/datalink.c
+++ b/sw/airborne/firmwares/fixedwing/datalink.c
@@ -47,10 +47,6 @@
#include "joystick.h"
#endif
-#ifdef USE_USB_SERIAL
-#include "mcu_periph/usb_serial.h"
-#endif
-
#ifdef HITL
#include "gps.h"
#endif
@@ -189,11 +185,20 @@ void dl_parse_msg(void) {
#if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK
if (msg_id == DL_RC_3CH /*&& DL_RC_3CH_ac_id(dl_buffer) == TX_ID*/) {
LED_TOGGLE(3);
- parse_rc_datalink(
+ parse_rc_3ch_datalink(
DL_RC_3CH_throttle_mode(dl_buffer),
DL_RC_3CH_roll(dl_buffer),
DL_RC_3CH_pitch(dl_buffer));
} else
+ if (msg_id == DL_RC_4CH && DL_RC_4CH_ac_id(dl_buffer) == AC_ID) {
+LED_TOGGLE(3);
+ parse_rc_4ch_datalink(
+ DL_RC_4CH_mode(dl_buffer),
+ DL_RC_4CH_throttle(dl_buffer),
+ DL_RC_4CH_roll(dl_buffer),
+ DL_RC_4CH_pitch(dl_buffer),
+ DL_RC_4CH_yaw(dl_buffer));
+ } else
#endif // RC_DATALINK
{ /* Last else */
/* Parse modules datalink */
diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c
index 839a0da20b..fcc14344f1 100644
--- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c
+++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c
@@ -57,7 +57,7 @@ float v_ctl_auto_throttle_pgain;
float v_ctl_auto_throttle_igain;
float v_ctl_auto_throttle_dgain;
float v_ctl_auto_throttle_sum_err;
-#define V_CTL_AUTO_THROTTLE_MAX_SUM_ERR 150
+#define V_CTL_AUTO_THROTTLE_MAX_SUM_ERR 0.3
float v_ctl_auto_throttle_pitch_of_vz_pgain;
float v_ctl_auto_throttle_pitch_of_vz_dgain;
@@ -70,14 +70,21 @@ float v_ctl_auto_pitch_pgain;
float v_ctl_auto_pitch_dgain;
float v_ctl_auto_pitch_igain;
float v_ctl_auto_pitch_sum_err;
-#define V_CTL_AUTO_PITCH_MAX_SUM_ERR 100
+#define V_CTL_AUTO_PITCH_MAX_SUM_ERR (RadOfDeg(10.))
+
+#ifndef V_CTL_AUTO_PITCH_DGAIN
+#define V_CTL_AUTO_PITCH_DGAIN 0.
+#endif
+#ifndef V_CTL_AUTO_PITCH_IGAIN
+#define V_CTL_AUTO_PITCH_IGAIN 0.
+#endif
float controlled_throttle;
pprz_t v_ctl_throttle_setpoint;
pprz_t v_ctl_throttle_slewed;
// Set higher than 2*V_CTL_ALTITUDE_MAX_CLIMB to disable
-#define V_CTL_AUTO_CLIMB_LIMIT 0.5/4.0 // m/s/s
+#define V_CTL_AUTO_CLIMB_LIMIT (0.5/4.0) // m/s/s
uint8_t v_ctl_speed_mode;
@@ -181,16 +188,21 @@ void v_ctl_altitude_loop( void ) {
static inline void v_ctl_set_pitch ( void ) {
static float last_err = 0.;
+ if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0)
+ v_ctl_auto_pitch_sum_err = 0;
+
// Compute errors
float err = estimator_z_dot - v_ctl_climb_setpoint;
float d_err = err - last_err;
last_err = err;
- v_ctl_auto_pitch_sum_err += err*(1./60.);
- BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR);
+ if (v_ctl_auto_pitch_igain < 0.) {
+ v_ctl_auto_pitch_sum_err += err*(1./60.);
+ BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR / (-v_ctl_auto_pitch_igain));
+ }
// PI loop + feedforward ctl
- nav_pitch = nav_pitch
+ nav_pitch = 0. //nav_pitch FIXME it really sucks !
+ v_ctl_auto_throttle_pitch_of_vz_pgain * v_ctl_climb_setpoint
+ v_ctl_auto_pitch_pgain * err
+ v_ctl_auto_pitch_dgain * d_err
@@ -201,13 +213,18 @@ static inline void v_ctl_set_pitch ( void ) {
static inline void v_ctl_set_throttle( void ) {
static float last_err = 0.;
+ if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0)
+ v_ctl_auto_throttle_sum_err = 0;
+
// Compute errors
float err = estimator_z_dot - v_ctl_climb_setpoint;
float d_err = err - last_err;
last_err = err;
- v_ctl_auto_throttle_sum_err += err*(1./60.);
- BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR);
+ if (v_ctl_auto_throttle_igain < 0.) {
+ v_ctl_auto_throttle_sum_err += err*(1./60.);
+ BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR / (-v_ctl_auto_throttle_igain));
+ }
// PID loop + feedforward ctl
controlled_throttle = v_ctl_auto_throttle_cruise_throttle
@@ -269,8 +286,7 @@ void v_ctl_climb_loop ( void ) {
}
// Set Throttle output
- float f_throttle = controlled_throttle;
- v_ctl_throttle_setpoint = TRIM_UPPRZ(f_throttle * MAX_PPRZ);
+ v_ctl_throttle_setpoint = TRIM_UPPRZ(controlled_throttle * MAX_PPRZ);
}
@@ -279,7 +295,7 @@ void v_ctl_climb_loop ( void ) {
#endif
#ifndef V_CTL_THROTTLE_SLEW
-#define V_CTL_THROTTLE_SLEW 1.
+#define V_CTL_THROTTLE_SLEW (1.)
#endif
/** \brief Computes slewed throttle from throttle setpoint
called at 20Hz
diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.h b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.h
index 4a6261d35b..d020a8cbb2 100644
--- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.h
+++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.h
@@ -39,9 +39,7 @@ extern float v_ctl_auto_pitch_dgain;
extern uint8_t v_ctl_speed_mode;
-#ifdef PITCH_TRIM
extern float v_ctl_pitch_loiter_trim;
extern float v_ctl_pitch_dash_trim;
-#endif
#endif /* FW_V_CTL_N_H */
diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
index 9ff099da95..5f571449e1 100644
--- a/sw/airborne/firmwares/fixedwing/main_ap.c
+++ b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -52,6 +52,7 @@
#include "sys_time.h"
#include "generated/flight_plan.h"
#include "datalink.h"
+#include "subsystems/settings.h"
#include "xbee.h"
#include "gpio.h"
@@ -551,6 +552,8 @@ void init_ap( void ) {
modules_init();
+ settings_init();
+
/** - start interrupt task */
mcu_int_enable();
diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c
index 4b138cd8b0..29b3489cc5 100644
--- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c
+++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c
@@ -60,11 +60,11 @@ float h_ctl_ref_roll_accel;
float h_ctl_ref_pitch_angle;
float h_ctl_ref_pitch_rate;
float h_ctl_ref_pitch_accel;
-#define H_CTL_REF_W 2.5
+#define H_CTL_REF_W 5.
#define H_CTL_REF_XI 0.85
-#define H_CTL_REF_MAX_P RadOfDeg(100.)
+#define H_CTL_REF_MAX_P RadOfDeg(150.)
#define H_CTL_REF_MAX_P_DOT RadOfDeg(500.)
-#define H_CTL_REF_MAX_Q RadOfDeg(100.)
+#define H_CTL_REF_MAX_Q RadOfDeg(150.)
#define H_CTL_REF_MAX_Q_DOT RadOfDeg(500.)
/* inner roll loop parameters */
@@ -96,21 +96,55 @@ float h_ctl_aileron_of_throttle;
float h_ctl_elevator_of_roll;
float h_ctl_pitch_of_roll; // Should be used instead of elevator_of_roll
+bool_t use_airspeed_ratio;
+float airspeed_ratio2;
+#define AIRSPEED_RATIO_MIN 0.5
+#define AIRSPEED_RATIO_MAX 2.
+
+float v_ctl_pitch_loiter_trim;
+float v_ctl_pitch_dash_trim;
inline static void h_ctl_roll_loop( void );
inline static void h_ctl_pitch_loop( void );
+// Some default course gains
+// H_CTL_COURSE_PGAIN needs to be define in airframe
#ifndef H_CTL_COURSE_PRE_BANK_CORRECTION
#define H_CTL_COURSE_PRE_BANK_CORRECTION 1.
#endif
-
#ifndef H_CTL_COURSE_DGAIN
#define H_CTL_COURSE_DGAIN 0.
#endif
+// Some default roll gains
+// H_CTL_ROLL_ATTITUDE_GAIN needs to be define in airframe
#ifndef H_CTL_ROLL_RATE_GAIN
#define H_CTL_ROLL_RATE_GAIN 0.
#endif
+#ifndef H_CTL_ROLL_IGAIN
+#define H_CTL_ROLL_IGAIN 0.
+#endif
+#ifndef H_CTL_ROLL_KFFA
+#define H_CTL_ROLL_KFFA 0.
+#endif
+#ifndef H_CTL_ROLL_KFFD
+#define H_CTL_ROLL_KFFD 0.
+#endif
+
+// Some default pitch gains
+// H_CTL_PITCH_PGAIN needs to be define in airframe
+#ifndef H_CTL_PITCH_DGAIN
+#define H_CTL_PITCH_DGAIN 0.
+#endif
+#ifndef H_CTL_PITCH_IGAIN
+#define H_CTL_PITCH_IGAIN 0.
+#endif
+#ifndef H_CTL_PITCH_KFFA
+#define H_CTL_PITCH_KFFA 0.
+#endif
+#ifndef H_CTL_PITCH_KFFD
+#define H_CTL_PITCH_KFFD 0.
+#endif
void h_ctl_init( void ) {
h_ctl_course_setpoint = 0.;
@@ -148,6 +182,17 @@ void h_ctl_init( void ) {
h_ctl_pitch_of_roll = H_CTL_PITCH_OF_ROLL;
#endif
+ use_airspeed_ratio = FALSE;
+ airspeed_ratio2 = 1.;
+
+#ifdef USE_PITCH_TRIM
+ v_ctl_pitch_loiter_trim = V_CTL_PITCH_LOITER_TRIM;
+ v_ctl_pitch_dash_trim = V_CTL_PITCH_DASH_TRIM;
+#else
+ v_ctl_pitch_loiter_trim = 0.;
+ v_ctl_pitch_dash_trim = 0.;
+#endif
+
}
/**
@@ -175,24 +220,27 @@ void h_ctl_course_loop ( void ) {
BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
}
-static float airspeed_ratio2;
-
+#ifdef USE_AIRSPEED
static inline void compute_airspeed_ratio( void ) {
- float throttle_diff = v_ctl_throttle_setpoint / (float)MAX_PPRZ - v_ctl_auto_throttle_nominal_cruise_throttle;
- float airspeed = NOMINAL_AIRSPEED; /* Estimated from the throttle */
- if (throttle_diff > 0)
- airspeed += throttle_diff / (V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE - v_ctl_auto_throttle_nominal_cruise_throttle) * (MAXIMUM_AIRSPEED - NOMINAL_AIRSPEED);
- else
- airspeed += throttle_diff / (v_ctl_auto_throttle_nominal_cruise_throttle - V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE) * (NOMINAL_AIRSPEED - MINIMUM_AIRSPEED);
-
+ if (use_airspeed_ratio) {
+ // low pass airspeed
+ static float airspeed = 0.;
+ airspeed = ( 15*airspeed + estimator_airspeed ) / 16;
+ // compute ratio
float airspeed_ratio = airspeed / NOMINAL_AIRSPEED;
- Bound(airspeed_ratio, 0.5, 2.);
+ Bound(airspeed_ratio, AIRSPEED_RATIO_MIN, AIRSPEED_RATIO_MAX);
airspeed_ratio2 = airspeed_ratio*airspeed_ratio;
+ } else {
+ airspeed_ratio2 = 1.;
+ }
}
+#endif
void h_ctl_attitude_loop ( void ) {
if (!h_ctl_disabled) {
- // compute_airspeed_ratio();
+#ifdef USE_AIRSPEED
+ compute_airspeed_ratio();
+#endif
h_ctl_roll_loop();
h_ctl_pitch_loop();
}
@@ -206,9 +254,7 @@ inline static void h_ctl_roll_loop( void ) {
static float cmd_fb = 0.;
- if (pprz_mode == PPRZ_MODE_MANUAL)
- h_ctl_roll_sum_err = 0;
-
+#ifdef USE_ANGLE_REF
// Update reference setpoints for roll
h_ctl_ref_roll_angle += h_ctl_ref_roll_rate * H_CTL_REF_DT;
h_ctl_ref_roll_rate += h_ctl_ref_roll_accel * H_CTL_REF_DT;
@@ -223,6 +269,11 @@ inline static void h_ctl_roll_loop( void ) {
h_ctl_ref_roll_rate = - H_CTL_REF_MAX_P;
if (h_ctl_ref_roll_accel < 0.) h_ctl_ref_roll_accel = 0.;
}
+#else
+ h_ctl_ref_roll_angle = h_ctl_roll_setpoint;
+ h_ctl_ref_roll_rate = 0.;
+ h_ctl_ref_roll_accel = 0.;
+#endif
#ifdef USE_KFF_UPDATE
// update Kff gains
@@ -242,26 +293,35 @@ inline static void h_ctl_roll_loop( void ) {
estimator_p = (err - last_err)/(1/60.);
last_err = err;
#endif
- float d_err = (estimator_p - h_ctl_ref_roll_rate) / H_CTL_REF_DT;
+ float d_err = estimator_p - h_ctl_ref_roll_rate;
- h_ctl_roll_sum_err += err * H_CTL_REF_DT;
- BoundAbs(h_ctl_roll_sum_err, H_CTL_ROLL_SUM_ERR_MAX);
+ if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
+ h_ctl_roll_sum_err = 0.;
+ }
+ else {
+ if (h_ctl_roll_igain < 0.) {
+ h_ctl_roll_sum_err += err * H_CTL_REF_DT;
+ BoundAbs(h_ctl_roll_sum_err, (- H_CTL_ROLL_SUM_ERR_MAX / h_ctl_roll_igain));
+ } else {
+ h_ctl_roll_sum_err = 0.;
+ }
+ }
- cmd_fb = h_ctl_roll_attitude_gain * err;// + h_ctl_roll_rate_gain * derr;
+ cmd_fb = h_ctl_roll_attitude_gain * err;// + h_ctl_roll_rate_gain * d_err;
float cmd = h_ctl_roll_Kffa * h_ctl_ref_roll_accel
+ h_ctl_roll_Kffd * h_ctl_ref_roll_rate
- cmd_fb
- h_ctl_roll_rate_gain * d_err
- h_ctl_roll_igain * h_ctl_roll_sum_err
+ v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;
- //float cmd = h_ctl_roll_Kffp * h_ctl_ref_roll_accel
- // + h_ctl_roll_Kffd * h_ctl_ref_roll_rate
- // - h_ctl_roll_attitude_gain * err
- // - h_ctl_roll_rate_gain * derr
- // - h_ctl_roll_igain * h_ctl_roll_sum_err
- // + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;
-
- //x cmd /= airspeed_ratio2;
+// float cmd = h_ctl_roll_Kffa * h_ctl_ref_roll_accel
+// + h_ctl_roll_Kffd * h_ctl_ref_roll_rate
+// - h_ctl_roll_attitude_gain * err
+// - h_ctl_roll_rate_gain * d_err
+// - h_ctl_roll_igain * h_ctl_roll_sum_err
+// + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;
+//
+ cmd /= airspeed_ratio2;
// Set aileron commands
h_ctl_aileron_setpoint = TRIM_PPRZ(cmd);
@@ -274,13 +334,17 @@ float v_ctl_auto_throttle_loiter_trim = V_CTL_AUTO_THROTTLE_LOITER_TRIM;
float v_ctl_auto_throttle_dash_trim = V_CTL_AUTO_THROTTLE_DASH_TRIM;
#endif
-#ifdef PITCH_TRIM
-float v_ctl_pitch_loiter_trim = V_CTL_PITCH_LOITER_TRIM;
-float v_ctl_pitch_dash_trim = V_CTL_PITCH_DASH_TRIM;
-
+#ifdef USE_PITCH_TRIM
inline static void loiter(void) {
float pitch_trim;
+#ifdef USE_AIRSPEED
+ if (estimator_airspeed > NOMINAL_AIRSPEED) {
+ pitch_trim = v_ctl_pitch_dash_trim * (airspeed_ratio2-1) / ((AIRSPEED_RATIO_MAX * AIRSPEED_RATIO_MAX) - 1);
+ } else {
+ pitch_trim = v_ctl_pitch_loiter_trim * (airspeed_ratio2-1) / ((AIRSPEED_RATIO_MIN * AIRSPEED_RATIO_MIN) - 1);
+ }
+#else
float throttle_diff = v_ctl_throttle_setpoint / (float)MAX_PPRZ - v_ctl_auto_throttle_nominal_cruise_throttle;
if (throttle_diff > 0) {
float max_diff = Max(V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE - v_ctl_auto_throttle_nominal_cruise_throttle, 0.1);
@@ -289,6 +353,7 @@ inline static void loiter(void) {
float max_diff = Max(v_ctl_auto_throttle_nominal_cruise_throttle - V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, 0.1);
pitch_trim = -throttle_diff / max_diff * v_ctl_pitch_loiter_trim;
}
+#endif
h_ctl_pitch_loop_setpoint += pitch_trim;
}
@@ -301,18 +366,16 @@ inline static void h_ctl_pitch_loop( void ) {
if (h_ctl_pitch_of_roll <0.)
h_ctl_pitch_of_roll = 0.;
- if (pprz_mode == PPRZ_MODE_MANUAL)
- h_ctl_pitch_sum_err = 0;
-
h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_pitch_of_roll * fabs(estimator_phi);
-#ifdef PITCH_TRIM
+#ifdef USE_PITCH_TRIM
loiter();
#endif
+#ifdef USE_ANGLE_REF
// Update reference setpoints for pitch
- h_ctl_ref_pitch_accel = H_CTL_REF_W*H_CTL_REF_W * (h_ctl_pitch_loop_setpoint - h_ctl_ref_pitch_angle) - 2*H_CTL_REF_XI*H_CTL_REF_W * h_ctl_ref_pitch_rate;
- h_ctl_ref_pitch_rate += h_ctl_ref_pitch_accel * H_CTL_REF_DT;
h_ctl_ref_pitch_angle += h_ctl_ref_pitch_rate * H_CTL_REF_DT;
+ h_ctl_ref_pitch_rate += h_ctl_ref_pitch_accel * H_CTL_REF_DT;
+ h_ctl_ref_pitch_accel = H_CTL_REF_W*H_CTL_REF_W * (h_ctl_pitch_loop_setpoint - h_ctl_ref_pitch_angle) - 2*H_CTL_REF_XI*H_CTL_REF_W * h_ctl_ref_pitch_rate;
// Saturation on references
BoundAbs(h_ctl_ref_pitch_accel, H_CTL_REF_MAX_Q_DOT);
if (h_ctl_ref_pitch_rate > H_CTL_REF_MAX_Q) {
@@ -323,14 +386,28 @@ inline static void h_ctl_pitch_loop( void ) {
h_ctl_ref_pitch_rate = - H_CTL_REF_MAX_Q;
if (h_ctl_ref_pitch_accel < 0.) h_ctl_ref_pitch_accel = 0.;
}
+#else
+ h_ctl_ref_pitch_angle = h_ctl_pitch_loop_setpoint;
+ h_ctl_ref_pitch_rate = 0.;
+ h_ctl_ref_pitch_accel = 0.;
+#endif
// Compute errors
float err = estimator_theta - h_ctl_ref_pitch_angle;
float d_err = (err - last_err)/H_CTL_REF_DT - h_ctl_ref_pitch_rate;
last_err = err;
- h_ctl_pitch_sum_err += err * H_CTL_REF_DT;
- BoundAbs(h_ctl_pitch_sum_err, H_CTL_ROLL_SUM_ERR_MAX);
+ if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
+ h_ctl_pitch_sum_err = 0.;
+ }
+ else {
+ if (h_ctl_pitch_igain < 0.) {
+ h_ctl_pitch_sum_err += err * H_CTL_REF_DT;
+ BoundAbs(h_ctl_pitch_sum_err, (- H_CTL_PITCH_SUM_ERR_MAX / h_ctl_pitch_igain));
+ } else {
+ h_ctl_pitch_sum_err = 0.;
+ }
+ }
float cmd = h_ctl_pitch_Kffa * h_ctl_ref_pitch_accel
+ h_ctl_pitch_Kffd * h_ctl_ref_pitch_rate
@@ -338,7 +415,8 @@ inline static void h_ctl_pitch_loop( void ) {
+ h_ctl_pitch_dgain * d_err
+ h_ctl_pitch_igain * h_ctl_pitch_sum_err;
- // cmd /= airspeed_ratio2;
+ cmd /= airspeed_ratio2;
h_ctl_elevator_setpoint = TRIM_PPRZ(cmd);
}
+
diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h
index eb084c9811..4a7c082f22 100644
--- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h
+++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h
@@ -46,16 +46,16 @@ extern float h_ctl_pitch_Kffa;
extern float h_ctl_pitch_Kffd;
extern float h_ctl_pitch_of_roll;
-#define H_CTL_ROLL_SUM_ERR_MAX 100.
-#define H_CTL_PITCH_SUM_ERR_MAX 100.
+#define H_CTL_ROLL_SUM_ERR_MAX (MAX_PPRZ/2.)
+#define H_CTL_PITCH_SUM_ERR_MAX (MAX_PPRZ/2.)
#define fw_h_ctl_a_SetRollIGain(_gain) { \
- h_ctl_roll_sum_err = 0; \
+ h_ctl_roll_sum_err = 0.; \
h_ctl_roll_igain = _gain; \
}
#define fw_h_ctl_a_SetPitchIGain(_gain) { \
- h_ctl_pitch_sum_err = 0; \
+ h_ctl_pitch_sum_err = 0.; \
h_ctl_pitch_igain = _gain; \
}
@@ -69,4 +69,6 @@ extern float h_ctl_ref_pitch_angle;
extern float h_ctl_ref_pitch_rate;
extern float h_ctl_ref_pitch_accel;
+extern bool_t use_airspeed_ratio;
+
#endif /* FW_H_CTL_A_H */
diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/ADC.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/ADC.pde
new file mode 100644
index 0000000000..9f8473b7e2
--- /dev/null
+++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/ADC.pde
@@ -0,0 +1,71 @@
+// We are using an oversampling and averaging method to increase the ADC resolution
+// The theorical ADC resolution is now 11.7 bits. Now we store the ADC readings in float format
+void Read_adc_raw(void)
+{
+ int i;
+ uint16_t temp1;
+ uint8_t temp2;
+
+ // ADC readings...
+ for (i=0;i<6;i++)
+ {
+ do{
+ temp1= analog_buffer[sensors[i]]; // sensors[] maps sensors to correct order
+ temp2= analog_count[sensors[i]];
+ } while(temp1 != analog_buffer[sensors[i]]); // Check if there was an ADC interrupt during readings...
+
+ if (temp2>0) AN[i] = float(temp1)/float(temp2); // Check for divide by zero
+
+ }
+ // Initialization for the next readings...
+ for (int i=0;i<8;i++){
+ do{
+ analog_buffer[i]=0;
+ analog_count[i]=0;
+ } while(analog_buffer[i]!=0); // Check if there was an ADC interrupt during initialization...
+ }
+}
+
+float read_adc(int select)
+{
+ float temp;
+ if (SENSOR_SIGN[select]<0){
+ temp = (AN_OFFSET[select]-AN[select]);
+ return constrain(temp,-900,900); //Throw out nonsensical values
+ } else {
+ temp = (AN[select]-AN_OFFSET[select]);
+ return constrain(temp,-900,900);
+ }
+}
+
+//Activating the ADC interrupts.
+void Analog_Init(void)
+{
+ ADCSRA|=(1<=8) MuxSel=0;
+ ADMUX = (analog_reference << 6) | MuxSel;
+ // start the conversion
+ ADCSRA|= (1< 0.64f) {
+ renorm= .5 * (3-renorm); //eq.21
+ } else if (renorm < 100.0f && renorm > 0.01f) {
+ renorm= 1. / sqrt(renorm);
+#if PERFORMANCE_REPORTING == 1
+ renorm_sqrt_count++;
+#endif
+ } else {
+ problem = TRUE;
+#if PERFORMANCE_REPORTING == 1
+ renorm_blowup_count++;
+#endif
+ }
+ Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
+
+ renorm= Vector_Dot_Product(&temporary[1][0],&temporary[1][0]);
+ if (renorm < 1.5625f && renorm > 0.64f) {
+ renorm= .5 * (3-renorm); //eq.21
+ } else if (renorm < 100.0f && renorm > 0.01f) {
+ renorm= 1. / sqrt(renorm);
+#if PERFORMANCE_REPORTING == 1
+ renorm_sqrt_count++;
+#endif
+ } else {
+ problem = TRUE;
+#if PERFORMANCE_REPORTING == 1
+ renorm_blowup_count++;
+#endif
+ }
+ Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
+
+ renorm= Vector_Dot_Product(&temporary[2][0],&temporary[2][0]);
+ if (renorm < 1.5625f && renorm > 0.64f) {
+ renorm= .5 * (3-renorm); //eq.21
+ } else if (renorm < 100.0f && renorm > 0.01f) {
+ renorm= 1. / sqrt(renorm);
+#if PERFORMANCE_REPORTING == 1
+ renorm_sqrt_count++;
+#endif
+ } else {
+ problem = TRUE;
+#if PERFORMANCE_REPORTING == 1
+ renorm_blowup_count++;
+#endif
+ }
+ Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
+
+ if (problem) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down!
+ DCM_Matrix[0][0]= 1.0f;
+ DCM_Matrix[0][1]= 0.0f;
+ DCM_Matrix[0][2]= 0.0f;
+ DCM_Matrix[1][0]= 0.0f;
+ DCM_Matrix[1][1]= 1.0f;
+ DCM_Matrix[1][2]= 0.0f;
+ DCM_Matrix[2][0]= 0.0f;
+ DCM_Matrix[2][1]= 0.0f;
+ DCM_Matrix[2][2]= 1.0f;
+ problem = FALSE;
+ }
+}
+
+/**************************************************/
+void Drift_correction(void)
+{
+ //Compensation the Roll, Pitch and Yaw drift.
+ static float Scaled_Omega_P[3];
+ static float Scaled_Omega_I[3];
+ float Accel_magnitude;
+ float Accel_weight;
+ float Integrator_magnitude;
+ float tempfloat;
+
+ //*****Roll and Pitch***************
+
+ // Calculate the magnitude of the accelerometer vector
+ Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
+ Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
+ // Dynamic weighting of accelerometer info (reliability filter)
+ // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
+ Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1); //
+
+#if PERFORMANCE_REPORTING == 1
+ tempfloat = ((Accel_weight - 0.5) * 256.0f); //amount added was determined to give imu_health a time constant about twice the time constant of the roll/pitch drift correction
+ imu_health += tempfloat;
+ imu_health = constrain(imu_health,129,65405);
+#endif
+
+ Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
+ Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
+
+ Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
+ Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
+
+ //*****YAW***************
+
+ // Use GPS Ground course to correct yaw gyro drift
+ if(ground_speed>=SPEEDFILT)
+ {
+ COGX = cos(ToRad(ground_course));
+ COGY = sin(ToRad(ground_course));
+ errorCourse=(DCM_Matrix[0][0]*COGY) - (DCM_Matrix[1][0]*COGX); //Calculating YAW error
+ Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
+
+ Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
+ Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
+
+ Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
+ Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
+ }
+ // Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
+ Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I,Omega_I));
+ if (Integrator_magnitude > ToRad(300)) {
+ Vector_Scale(Omega_I,Omega_I,0.5f*ToRad(300)/Integrator_magnitude);
+ }
+
+}
+
+/**************************************************/
+void Accel_adjust(void)
+{
+ Accel_Vector[1] += Accel_Scale(speed_3d*Omega[2]); // Centrifugal force on Acc_y = GPS_speed*GyroZ
+ Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[1]); // Centrifugal force on Acc_z = GPS_speed*GyroY
+}
+
+/**************************************************/
+void Matrix_update(void)
+{
+ Gyro_Vector[0]=Gyro_Scaled_X(read_adc(0)); //gyro x roll
+ Gyro_Vector[1]=Gyro_Scaled_Y(read_adc(1)); //gyro y pitch
+ Gyro_Vector[2]=Gyro_Scaled_Z(read_adc(2)); //gyro Z yaw
+
+ Accel_Vector[0]=read_adc(3); // acc x
+ Accel_Vector[1]=read_adc(4); // acc y
+ Accel_Vector[2]=read_adc(5); // acc z
+
+ Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term
+ Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
+
+ Accel_adjust(); //Remove centrifugal acceleration.
+
+ Update_Matrix[0][0]=0;
+ Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
+ Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
+ Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
+ Update_Matrix[1][1]=0;
+ Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
+ Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
+ Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
+ Update_Matrix[2][2]=0;
+
+ Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
+
+ for(int x=0; x<3; x++) //Matrix Addition (update)
+ {
+ for(int y=0; y<3; y++)
+ {
+ DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
+ }
+ }
+}
+
+/**************************************************/
+void Euler_angles(void)
+{
+ pitch = -asin(DCM_Matrix[2][0]);
+ roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
+ yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
+}
+
diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_pprz.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_pprz.pde
new file mode 100644
index 0000000000..6a2ddf704d
--- /dev/null
+++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/GPS_pprz.pde
@@ -0,0 +1,64 @@
+/****************************************************************
+ * Parse GPS data from a Paparazzi autopilot
+ ****************************************************************/
+ // Code from Jordi, modified by Jose, modified by Gautier
+
+
+void init_gps(void)
+{
+ //Serial.begin(38400);
+ pinMode(2,OUTPUT); //Serial Mux
+ digitalWrite(2,HIGH); //Serial Mux
+}
+
+/****************************************************************
+ *
+ ****************************************************************/
+void parse_pprz_gps() {
+
+#if PERFORMANCE_REPORTING == 1
+ gps_pos_fix_count++;
+#endif
+
+ speed_3d = (float)join_4_bytes(&Paparazzi_GPS_buffer[0])/100.0; // m/s 0,1,2,3
+ ground_speed = (float)join_4_bytes(&Paparazzi_GPS_buffer[4])/100.0; // Ground speed 2D 4,5,6,7
+ ground_course = (float)join_4_bytes(&Paparazzi_GPS_buffer[8])/100000.0; // Heading 2D 8,9,10,11
+ stGpsFix = Paparazzi_GPS_buffer[12];
+ stFlags = Paparazzi_GPS_buffer[13];
+
+ if((stGpsFix >= 0x03) && (stFlags&0x01)) {
+ gpsFix = 0; //valid position
+ digitalWrite(6,HIGH); //Turn LED when gps is fixed.
+ GPS_timer = DIYmillis(); //Restarting timer...
+ }
+ else {
+ gpsFix = 1; //invalid position
+ digitalWrite(6,LOW);
+ }
+
+ if (ground_speed > SPEEDFILT && gpsFix==0) gc_offset = ground_course - ToDeg(yaw);
+
+}
+
+
+/****************************************************************
+ * Join 4 bytes into a long
+ ****************************************************************/
+int32_t join_4_bytes(byte Buffer[])
+{
+ longUnion.byte[0] = *Buffer;
+ longUnion.byte[1] = *(Buffer+1);
+ longUnion.byte[2] = *(Buffer+2);
+ longUnion.byte[3] = *(Buffer+3);
+ return(longUnion.dword);
+}
+
+/****************************************************************
+ *
+void checksum(byte ubx_data)
+{
+ ck_a+=ubx_data;
+ ck_b+=ck_a;
+}
+
+ ****************************************************************/
diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Output.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Output.pde
new file mode 100644
index 0000000000..2ee09a3d7c
--- /dev/null
+++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Output.pde
@@ -0,0 +1,203 @@
+
+//*****I2C Output************************************************************
+void fill_I2C_message() {
+ // Message Array : Roll; Pitch; YaW; GyroX; GyroY; GyroZ; ACCX; ACCY; ACCZ
+ // Float Number is multipited with 10000 and converted to an Integer, for sending via I2C.
+ // Resolution for angles: 12, rates: 12, accel:10 (from pprza BFP math lib)
+ I2C_Message_ar[0] = int(roll*(1<<12));
+ I2C_Message_ar[1] = int(pitch*(1<<12));
+ I2C_Message_ar[2] = int(yaw*(1<<12));
+ I2C_Message_ar[3] = int(Omega_Vector[0]*(1<<12));
+ I2C_Message_ar[4] = int(Omega_Vector[1]*(1<<12));
+ I2C_Message_ar[5] = int(Omega_Vector[2]*(1<<12));
+ I2C_Message_ar[6] = int((9.81*Accel_Vector[0]/GRAVITY)*(1<<10));
+ I2C_Message_ar[7] = int((9.81*Accel_Vector[1]/GRAVITY)*(1<<10));
+ I2C_Message_ar[8] = int((9.81*Accel_Vector[2]/GRAVITY)*(1<<10));
+
+}
+
+void requestEvent(){
+#if PRINT_DEBUG != 0
+ Serial.println("Sending IMU Data");
+#endif
+
+ fill_I2C_message();
+
+ byte* pointer;
+ pointer = (byte*) &I2C_Message_ar;
+ Wire.send(pointer, 18);
+
+}
+
+//******** GPS Data from Paparazzi AP ******************************************
+
+void receiveEvent(int howMany){
+#if PRINT_DEBUG != 0
+ Serial.print("Receiving GPS Bytes : ");
+ Serial.println(howMany);
+#endif
+
+ for(int i=0; i < howMany; i++){
+ Paparazzi_GPS_buffer[i]=Wire.receive();
+ }
+
+ parse_pprz_gps(); // Parse new GPS packet...
+ GPS_timer=DIYmillis(); //Restarting timer...
+
+ gpsDataReady=1;
+
+}
+
+
+//*************************************************************************************************************
+
+
+void printdata(void){
+
+#if PRINT_I2C_MSG == 1
+ fill_I2C_message();
+ Serial.print("Time ");
+ Serial.print(millis());
+ Serial.print("; Roll ");
+ Serial.print(I2C_Message_ar[0]);
+ Serial.print("; Pitch ");
+ Serial.print(I2C_Message_ar[1]);
+ Serial.print("; YaW ");
+ Serial.print(I2C_Message_ar[2]);
+ Serial.print("; GyroX ");
+ Serial.print(I2C_Message_ar[3]);
+ Serial.print("; GyroY ");
+ Serial.print(I2C_Message_ar[4]);
+ Serial.print("; GyroZ ");
+ Serial.print(I2C_Message_ar[5]);
+ Serial.print("; ACCX ");
+ Serial.print(I2C_Message_ar[6]);
+ Serial.print("; ACCY ");
+ Serial.print(I2C_Message_ar[7]);
+ Serial.print("; ACCZ ");
+ Serial.println(I2C_Message_ar[8]);
+#endif
+
+ //Serial.print("!!!VER:");
+ //Serial.print(SOFTWARE_VER); //output the software version
+ //Serial.print(",");
+
+#if PRINT_ANALOGS == 1
+ Serial.print("AN0:");
+ Serial.print(read_adc(0)); //Reversing the sign.
+ Serial.print(",AN1:");
+ Serial.print(read_adc(1));
+ Serial.print(",AN2:");
+ Serial.print(read_adc(2));
+ Serial.print(",AN3:");
+ Serial.print(read_adc(3));
+ Serial.print (",AN4:");
+ Serial.print(read_adc(4));
+ Serial.print (",AN5:");
+ Serial.println(read_adc(5));
+#endif
+
+#if PRINT_DCM == 1
+ Serial.print ("EX0:");
+ Serial.print(convert_to_dec(DCM_Matrix[0][0]));
+ Serial.print (",EX1:");
+ Serial.print(convert_to_dec(DCM_Matrix[0][1]));
+ Serial.print (",EX2:");
+ Serial.print(convert_to_dec(DCM_Matrix[0][2]));
+ Serial.print (",EX3:");
+ Serial.print(convert_to_dec(DCM_Matrix[1][0]));
+ Serial.print (",EX4:");
+ Serial.print(convert_to_dec(DCM_Matrix[1][1]));
+ Serial.print (",EX5:");
+ Serial.print(convert_to_dec(DCM_Matrix[1][2]));
+ Serial.print (",EX6:");
+ Serial.print(convert_to_dec(DCM_Matrix[2][0]));
+ Serial.print (",EX7:");
+ Serial.print(convert_to_dec(DCM_Matrix[2][1]));
+ Serial.print (",EX8:");
+ Serial.println(convert_to_dec(DCM_Matrix[2][2]));
+#endif
+
+#if PRINT_EULER == 1
+ Serial.print("RLL:");
+ Serial.print(ToDeg(roll));
+ Serial.print(",PCH:");
+ Serial.print(ToDeg(pitch));
+ Serial.print(",YAW:");
+ Serial.print(ToDeg(yaw));
+ Serial.print(",IMUH:");
+ Serial.println((imu_health>>8)&0xff);
+#endif
+
+#if PRINT_GPS == 1
+ if(gpsFixnew==1) {
+ gpsFixnew=0;
+ Serial.print("COG:");
+ Serial.print((ground_course));
+ Serial.print(",SOG:");
+ Serial.print(ground_speed);
+ Serial.print(",FIX:");
+ Serial.print((int)gpsFix);
+ Serial.print (",");
+#if PERFORMANCE_REPORTING == 1
+ gps_messages_sent++;
+#endif
+ }
+ Serial.println("");
+#endif
+
+}
+
+void printPerfData(long time)
+{
+ Serial.print("PPP");
+ Serial.print("pTm:");
+ Serial.print(time,DEC);
+ Serial.print(",mLc:");
+ Serial.print(mainLoop_count,DEC);
+ Serial.print(",DtM:");
+ Serial.print(G_Dt_max,DEC);
+ Serial.print(",gsc:");
+ Serial.print(gyro_sat_count,DEC);
+ Serial.print(",adc:");
+ Serial.print(adc_constraints,DEC);
+ Serial.print(",rsc:");
+ Serial.print(renorm_sqrt_count,DEC);
+ Serial.print(",rbc:");
+ Serial.print(renorm_blowup_count,DEC);
+ Serial.print(",gpe:");
+ Serial.print(gps_payload_error_count,DEC);
+ Serial.print(",gce:");
+ Serial.print(gps_checksum_error_count,DEC);
+ Serial.print(",gpf:");
+ Serial.print(gps_pos_fix_count,DEC);
+ Serial.print(",gnf:");
+ Serial.print(gps_nav_fix_count,DEC);
+ Serial.print(",gms:");
+ Serial.print(gps_messages_sent,DEC);
+ Serial.print(",imu:");
+ Serial.print((imu_health>>8),DEC);
+ Serial.println(",***");
+
+ // Reset counters
+ mainLoop_count = 0;
+ G_Dt_max = 0;
+ gyro_sat_count = 0;
+ adc_constraints = 0;
+ renorm_sqrt_count = 0;
+ renorm_blowup_count = 0;
+ gps_payload_error_count = 0;
+ gps_checksum_error_count = 0;
+ gps_pos_fix_count = 0;
+ gps_nav_fix_count = 0;
+ gps_messages_sent = 0;
+
+
+}
+
+
+long convert_to_dec(float x)
+{
+ return x*10000000;
+}
+
diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Vector.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Vector.pde
new file mode 100644
index 0000000000..3c78b874db
--- /dev/null
+++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Vector.pde
@@ -0,0 +1,40 @@
+//Computes the dot product of two vectors
+float Vector_Dot_Product(float vector1[3],float vector2[3])
+{
+ float op=0;
+
+ for(int c=0; c<3; c++)
+ {
+ op+=vector1[c]*vector2[c];
+ }
+
+ return op;
+}
+
+//Computes the cross product of two vectors
+void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3])
+{
+ vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]);
+ vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]);
+ vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]);
+}
+
+//Multiply the vector by a scalar.
+void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2)
+{
+ for(int c=0; c<3; c++)
+ {
+ vectorOut[c]=vectorIn[c]*scale2;
+ }
+}
+
+void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3])
+{
+ for(int c=0; c<3; c++)
+ {
+ vectorOut[c]=vectorIn1[c]+vectorIn2[c];
+ }
+}
+
+
+
diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/arduimu_basic.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/arduimu_basic.pde
new file mode 100644
index 0000000000..7c5f550e0a
--- /dev/null
+++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/arduimu_basic.pde
@@ -0,0 +1,547 @@
+// Released under Creative Commons License
+// Code by Jordi Munoz and William Premerlani, Supported by Chris Anderson (Wired) and Nathan Sindle (SparkFun).
+// Version 1.0 for flat board updated by Doug Weibel and Jose Julio
+// Version 1.7 includes support for SCP1000 absolute pressure sensor
+// Version modified by Gautier Hattenberger for use with Paparazzi autopilot only (no barometer, no compass)
+
+// Axis definition: X axis pointing forward, Y axis pointing to the right and Z axis pointing down.
+// Positive pitch : nose up
+// Positive roll : right wing down
+// Positive yaw : clockwise
+
+#include
+#include
+
+//**********************************************************************
+// This section contains USER PARAMETERS !!!
+//
+//**********************************************************************
+
+// *** NOTE! Hardware version - Can be used for v1 (daughterboards) or v2 (flat)
+#define BOARD_VERSION 2 // 1 For V1 and 2 for V2
+
+// Enable Air Start uses Remove Before Fly flag - connection to pin 6 on ArduPilot
+#define ENABLE_AIR_START 1 // 1 if using airstart/groundstart signaling, 0 if not
+#define GROUNDSTART_PIN 8 // Pin number used for ground start signal (recommend 10 on v1 and 8 on v2 hardware)
+
+/*Min Speed Filter for Yaw drift Correction*/
+#define SPEEDFILT 2 // >1 use min speed filter for yaw drift cancellation, 0=do not use speed filter
+
+/*For debugging propurses*/
+#define PRINT_DEBUG 0 //Will print Debug messages
+
+#define PRINT_I2C_MSG 0 //Will print the I2C output buffer
+#define PRINT_DCM 0 //Will print the whole direction cosine matrix
+#define PRINT_ANALOGS 0 //Will print the analog raw data
+#define PRINT_EULER 0 //Will print the Euler angles Roll, Pitch and Yaw
+#define PRINT_GPS 0 //Will print GPS data
+
+
+
+
+//**********I2C Parameter ********************************************
+int I2C_Message_ar[9];
+
+//********************************************************************
+
+
+// *** NOTE! To use ArduIMU with ArduPilot you must select binary output messages (change to 1 here)
+#define PRINT_BINARY 1 //Will print binary message and suppress ASCII messages (above)
+
+// *** NOTE! Performance reporting is only supported for Ublox. Set to 0 for others
+#define PERFORMANCE_REPORTING 1 //Will include performance reports in the binary output ~ 1/2 min
+
+//**********************************************************************
+// End of user parameters
+//**********************************************************************
+
+#define SOFTWARE_VER "1.7"
+
+// ADC : Voltage reference 3.3v / 10bits(1024 steps) => 3.22mV/ADC step
+// ADXL335 Sensitivity(from datasheet) => 330mV/g, 3.22mV/ADC step => 330/3.22 = 102.48
+// Tested value : 101
+#define GRAVITY 101 //this equivalent to 1G in the raw data coming from the accelerometer
+#define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square
+
+#define ToRad(x) (x*0.01745329252) // *pi/180
+#define ToDeg(x) (x*57.2957795131) // *180/pi
+
+// LPR530 & LY530 Sensitivity (from datasheet) => 3.33mV/º/s, 3.22mV/ADC step => 1.03
+#define Gyro_Gain_X 1.0 //X axis Gyro gain
+#define Gyro_Gain_Y 1.0 //Y axis Gyro gain
+#define Gyro_Gain_Z 1.0 //Z axis Gyro gain
+#define Gyro_Scaled_X(x) x*ToRad(Gyro_Gain_X) //Return the scaled ADC raw data of the gyro in radians for second
+#define Gyro_Scaled_Y(x) x*ToRad(Gyro_Gain_Y) //Return the scaled ADC raw data of the gyro in radians for second
+#define Gyro_Scaled_Z(x) x*ToRad(Gyro_Gain_Z) //Return the scaled ADC raw data of the gyro in radians for second
+
+#define Kp_ROLLPITCH 0.015
+#define Ki_ROLLPITCH 0.000010
+#define Kp_YAW 1.2
+//#define Kp_YAW 2.5 //High yaw drift correction gain - use with caution!
+#define Ki_YAW 0.00005
+
+#define ADC_WARM_CYCLES 100
+
+#define FALSE 0
+#define TRUE 1
+
+
+float G_Dt=0.02; // Integration time (DCM algorithm)
+
+long timeNow=0; // Hold the milliseond value for now
+long timer=0; //general purpuse timer
+long timer_old;
+long timer24=0; //Second timer used to print values
+boolean groundstartDone = false; // Used to not repeat ground start
+
+float AN[8]; //array that store the 6 ADC filtered data
+float AN_OFFSET[8]; //Array that stores the Offset of the gyros
+
+float Accel_Vector[3]= {0,0,0}; //Store the acceleration in a vector
+float Gyro_Vector[3]= {0,0,0};//Store the gyros rutn rate in a vector
+float Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data
+float Omega_P[3]= {0,0,0};//Omega Proportional correction
+float Omega_I[3]= {0,0,0};//Omega Integrator
+float Omega[3]= {0,0,0};
+
+//Magnetometer variables
+//int magnetom_x;
+//int magnetom_y;
+//int magnetom_z;
+//float MAG_Heading;
+
+// Euler angles
+float roll;
+float pitch;
+float yaw;
+
+float errorRollPitch[3]= {0,0,0};
+float errorYaw[3]= {0,0,0};
+float errorCourse=180;
+float COGX=0; //Course overground X axis
+float COGY=1; //Course overground Y axis
+
+unsigned int cycleCount=0;
+byte gyro_sat=0;
+
+float DCM_Matrix[3][3] = {
+ { 1,0,0 },
+ { 0,1,0 },
+ { 0,0,1 }
+};
+
+float Update_Matrix[3][3] = {
+ { 0,1,2 },
+ { 3,4,5 },
+ { 6,7,8 }
+}; //Gyros here
+
+float Temporary_Matrix[3][3]={
+ { 0,0,0 },
+ { 0,0,0 },
+ { 0,0,0 }
+};
+
+//GPS
+
+//GPS stuff
+union long_union {
+ int32_t dword;
+ uint8_t byte[4];
+} longUnion;
+
+union int_union {
+ int16_t word;
+ uint8_t byte[2];
+} intUnion;
+
+/*Flight GPS variables*/
+int gpsFix=1; //This variable store the status of the GPS
+int gpsFixnew=0; //used to flag when new gps data received - used for binary output message flags
+int gps_fix_count = 5; //used to count 5 good fixes at ground startup
+float speed_3d=0; //Speed (3-D)
+float ground_speed=0;// This is the velocity your "plane" is traveling in meters for second, 1Meters/Second= 3.6Km/H = 1.944 knots
+float ground_course=90;//This is the runaway direction of you "plane" in degrees
+float gc_offset = 0; // Force yaw output to ground course when fresh data available (only implemented for ublox&binary message)
+unsigned long GPS_timer=0;
+
+//***********************GPS PAPARAZZI************************************************************************
+#define PPRZ_MAXPAYLOAD 32
+byte Paparazzi_GPS_buffer[PPRZ_MAXPAYLOAD];
+int gpsDataReady = 0; // sind neuen GPS daten vorhanden ??
+byte stGpsFix;
+byte stFlags;
+byte messageNr;
+//************************************************************************************************************
+
+//ADC variables
+volatile uint8_t MuxSel=0;
+volatile uint8_t analog_reference = DEFAULT;
+volatile uint16_t analog_buffer[8];
+volatile uint8_t analog_count[8];
+
+
+#if BOARD_VERSION == 1
+uint8_t sensors[6] = {0,2,1,3,5,4}; // Use these two lines for Hardware v1 (w/ daughterboards)
+int SENSOR_SIGN[]= {1,-1,1,-1,1,-1,-1,-1,-1}; //Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ
+#endif
+
+#if BOARD_VERSION == 2
+uint8_t sensors[6] = {6,7,3,0,1,2}; // For Hardware v2 flat
+int SENSOR_SIGN[] = {1,-1,-1,1,-1,1,-1,-1,-1};
+#endif
+
+// Performance Monitoring variables
+// Data collected and reported for ~1/2 minute intervals
+#if PERFORMANCE_REPORTING == 1
+int mainLoop_count = 0; //Main loop cycles since last report
+int G_Dt_max = 0.0; //Max main loop cycle time in milliseconds
+byte gyro_sat_count = 0;
+byte adc_constraints = 0;
+byte renorm_sqrt_count = 0;
+byte renorm_blowup_count = 0;
+byte gps_payload_error_count = 0;
+byte gps_checksum_error_count = 0;
+byte gps_pos_fix_count = 0;
+byte gps_nav_fix_count = 0;
+byte gps_messages_sent = 0;
+long perf_mon_timer = 0;
+#endif
+unsigned int imu_health = 65012;
+
+//******************************************************************
+void setup()
+{
+ Serial.begin(38400);
+ pinMode(2,OUTPUT); //Serial Mux
+ digitalWrite(2,HIGH); //Serial Mux
+ pinMode(5,OUTPUT); //Red LED
+ pinMode(6,OUTPUT); // Blue LED
+ pinMode(7,OUTPUT); // Yellow LED
+ pinMode(GROUNDSTART_PIN,INPUT); // Remove Before Fly flag (pin 6 on ArduPilot)
+ digitalWrite(GROUNDSTART_PIN,HIGH);
+
+ //************Define I2C Output Handler************************
+ Wire.begin(17); // join i2c bus with address #2
+ Wire.onRequest(requestEvent);
+ Wire.onReceive(receiveEvent); // register event --> Output
+ //*************************************************************
+
+ Analog_Reference(EXTERNAL); //Using external analog reference
+ Analog_Init();
+
+ debug_print("Welcome...");
+
+#if BOARD_VERSION == 1
+ debug_print("You are using Hardware Version 1...");
+#endif
+
+#if BOARD_VERSION == 2
+ debug_print("You are using Hardware Version 2...");
+#endif
+
+ //Serial.print("ArduIMU: v");
+ //Serial.println(SOFTWARE_VER);
+ debug_handler(0);//Printing version
+
+ if(ENABLE_AIR_START) {
+ debug_handler(1);
+ startup_air();
+ } else {
+ debug_handler(2);
+ startup_ground();
+ }
+
+ delay(250);
+ Read_adc_raw(); // ADC initialization
+ timer=DIYmillis();
+ delay(20);
+
+}
+
+//***************************************************************************************
+void loop() //Main Loop
+{
+ timeNow = millis();
+
+ // 20 -> Main loop runs at 50Hz
+ // 5 -> Main loop runs at 200Hz
+ // Max measured duty time around 3ms
+ if((timeNow-timer)>=5)
+ {
+ timer_old = timer;
+ timer = timeNow;
+
+#if PERFORMANCE_REPORTING == 1
+ mainLoop_count++;
+ if (timer-timer_old > G_Dt_max) G_Dt_max = timer-timer_old;
+#endif
+
+ G_Dt = (timer-timer_old)/1000.0; // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
+ if(G_Dt > 1) G_Dt = 0; //Something is wrong - keeps dt from blowing up, goes to zero to keep gyros from departing
+
+ // *** DCM algorithm
+
+ Read_adc_raw();
+
+ Matrix_update();
+
+ Normalize();
+
+ Drift_correction();
+
+ Euler_angles();
+
+#if PRINT_BINARY == 1
+ printdata(); //Send info via serial
+#endif
+
+ //Turn on the LED when you saturate any of the gyros.
+ if((abs(Gyro_Vector[0])>=ToRad(300))||(abs(Gyro_Vector[1])>=ToRad(300))||(abs(Gyro_Vector[2])>=ToRad(300)))
+ {
+ gyro_sat=1;
+#if PERFORMANCE_REPORTING == 1
+ gyro_sat_count++;
+#endif
+ digitalWrite(5,HIGH);
+ }
+
+ cycleCount++;
+
+ // Do these things every 6th time through the main cycle
+ // This section gets called every 1000/(20*6) = 8 1/3 Hz
+ // doing it this way removes the need for another 'millis()' call
+ // and balances the processing load across main loop cycles.
+ switch (cycleCount) {
+ case(0):
+ if(DIYmillis() - GPS_timer > 2000) {
+ digitalWrite(6, LOW); //If we don't receive any byte in two seconds turn off gps fix LED...
+ debug_print("No GPS data");
+ gpsFix = 1;
+ }
+ break;
+
+ case(1):
+ //Here we will check if we are getting a signal to ground start
+ if (digitalRead(GROUNDSTART_PIN) == LOW && groundstartDone == false) startup_ground();
+ break;
+
+ case(2):
+ // GYRO Saturation indication
+ if(gyro_sat>=1) {
+ digitalWrite(5,HIGH); //Turn Red LED when gyro is saturated.
+ if(gyro_sat>=8) // keep the LED on for 8/10ths of a second
+ gyro_sat=0;
+ else
+ gyro_sat++;
+ } else {
+ digitalWrite(5,LOW);
+ }
+ break;
+
+ case(3):
+ // YAW drift correction indication
+ if(ground_speed 20000) {
+ printPerfData(timeNow-perf_mon_timer);
+ perf_mon_timer=timeNow;
+ }
+#endif
+
+ }
+
+}
+
+//********************************************************************************
+void startup_ground(void)
+{
+ uint16_t store=0;
+ int flashcount = 0;
+
+ debug_handler(2);
+ for(int c=0; c= 10) {
+ flashcount = 0;
+ digitalWrite(7,HIGH);
+ digitalWrite(6,LOW);
+ digitalWrite(5,HIGH);
+ }
+ flashcount++;
+ }
+ digitalWrite(5,LOW);
+ digitalWrite(6,LOW);
+ digitalWrite(7,LOW);
+
+ AN_OFFSET[5]-=GRAVITY*SENSOR_SIGN[5];
+
+ // Store parameters in eeprom
+ for(int y=0; y<=5; y++)
+ {
+ Serial.println(AN_OFFSET[y]);
+ store = ((AN_OFFSET[y]-200.f)*100.0f);
+ eeprom_busy_wait();
+ eeprom_write_word((uint16_t *) (y*2+2), store);
+ }
+
+ groundstartDone = true;
+ debug_handler(6);
+}
+
+//************************************************************************************
+void startup_air(void)
+{
+ uint16_t temp=0;
+
+ for(int y=0; y<=5; y++)
+ {
+ eeprom_busy_wait();
+ temp = eeprom_read_word((uint16_t *) (y*2+2));
+ AN_OFFSET[y] = temp/100.f+200.f;
+ Serial.println(AN_OFFSET[y]);
+ }
+ debug_handler(5);
+}
+
+
+void debug_print(char string[])
+{
+#if PRINT_DEBUG != 0
+ Serial.print("???");
+ Serial.print(string);
+ Serial.println("***");
+#endif
+}
+
+void debug_handler(byte message)
+{
+#if PRINT_DEBUG != 0
+
+ static unsigned long BAD_Checksum=0;
+
+ switch(message)
+ {
+ case 0:
+ Serial.print("???Software Version ");
+ Serial.print(SOFTWARE_VER);
+ Serial.println("***");
+ break;
+
+ case 1:
+ Serial.println("???Air Start!***");
+ break;
+
+ case 2:
+ Serial.println("???Ground Start!***");
+ break;
+
+ case 3:
+ Serial.println("???Enabling Magneto...***");
+ break;
+
+ case 4:
+ Serial.println("???Enabling Pressure Altitude...***");
+ break;
+
+ case 5:
+ Serial.println("???Air Start complete");
+ break;
+
+ case 6:
+ Serial.println("???Ground Start complete");
+ break;
+
+ case 10:
+ BAD_Checksum++;
+ Serial.print("???GPS Bad Checksum: ");
+ Serial.print(BAD_Checksum);
+ Serial.println("...***");
+ break;
+
+ default:
+ Serial.println("???Invalid debug ID...***");
+ break;
+
+ }
+#endif
+
+}
+
+/*
+EEPROM memory map
+
+0 0x00 Unused
+1 0x01 ..
+2 0x02 AN_OFFSET[0]
+3 0x03 ..
+4 0x04 AN_OFFSET[1]
+5 0x05 ..
+6 0x06 AN_OFFSET[2]
+7 0x07 ..
+8 0x08 AN_OFFSET[3]
+9 0x09 ..
+10 0x0A AN_OFFSET[4]
+11 0x0B ..
+12 0x0C AN_OFFSET[5]
+13 0x0D ..
+14 0x0E Unused
+15 0x0F ..
+16 0x10 Ground Pressure
+17 0x11 ..
+18 0x12 ..
+19 0x13 ..
+20 0x14 Ground Temp
+21 0x15 ..
+22 0x16 Ground Altitude
+23 0x17 ..
+*/
diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/matrix.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/matrix.pde
new file mode 100644
index 0000000000..fd825e0e30
--- /dev/null
+++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/matrix.pde
@@ -0,0 +1,22 @@
+/**************************************************/
+//Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!).
+void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3])
+{
+ float op[3];
+ for(int x=0; x<3; x++)
+ {
+ for(int y=0; y<3; y++)
+ {
+ for(int w=0; w<3; w++)
+ {
+ op[w]=a[x][w]*b[w][y];
+ }
+ mat[x][y]=0;
+ mat[x][y]=op[0]+op[1]+op[2];
+
+ float test=mat[x][y];
+ }
+ }
+}
+
+
diff --git a/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/timing.pde b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/timing.pde
new file mode 100644
index 0000000000..5c736e5518
--- /dev/null
+++ b/sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/timing.pde
@@ -0,0 +1,23 @@
+extern unsigned long timer0_millis;
+
+// this function replaces the arduino millis() funcion
+unsigned long DIYmillis()
+{
+ unsigned long m;
+ unsigned long m2;
+
+ // timer0_millis could change inside timer0 interrupt and we don´t want to disable interrupts
+ // we can do two readings and compare.
+ m = timer0_millis;
+ m2 = timer0_millis;
+ if (m!=m2) // timer0_millis corrupted?
+ m = timer0_millis; // this should be fine...
+ return m;
+}
+
+void DIYdelay(unsigned long ms)
+{
+ unsigned long start = DIYmillis();
+ while (DIYmillis() - start <= ms)
+ ;
+}
diff --git a/sw/airborne/booz/arch/sim/booz2_analog_hw.c b/sw/airborne/firmwares/rotorcraft/actuators/actuators_dummy.c
similarity index 87%
rename from sw/airborne/booz/arch/sim/booz2_analog_hw.c
rename to sw/airborne/firmwares/rotorcraft/actuators/actuators_dummy.c
index 25404f6ec4..3204727e9f 100644
--- a/sw/airborne/booz/arch/sim/booz2_analog_hw.c
+++ b/sw/airborne/firmwares/rotorcraft/actuators/actuators_dummy.c
@@ -21,9 +21,15 @@
* Boston, MA 02111-1307, USA.
*/
-#include "booz2_analog.h"
+#include "firmwares/rotorcraft/actuators.h"
-void booz2_analog_init_hw(void) {
+
+
+
+void actuators_init(void) {
}
+
+void actuators_set(bool_t motors_on) {
+}
diff --git a/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.c b/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.c
index 90bb98f8f9..07602f1430 100644
--- a/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.c
+++ b/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.c
@@ -43,7 +43,6 @@ void actuators_init(void) {
actuators_mkk.trans[i].type = I2CTransTx;
actuators_mkk.trans[i].len_w = 1;
actuators_mkk.trans[i].slave_addr = actuators_addr[i];
- actuators_mkk.trans[i].stop_after_transmit = TRUE;
actuators_mkk.trans[i].status = I2CTransSuccess;
}
diff --git a/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm.h b/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm.h
index a38ed0b6e2..6d17dc4561 100644
--- a/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm.h
+++ b/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm.h
@@ -27,12 +27,13 @@
#include "std.h"
#include "firmwares/rotorcraft/actuators.h"
-#define ACTUATORS_PWM_NB 6
+#include BOARD_CONFIG
+#include "actuators_pwm_arch.h"
+
extern int32_t actuators_pwm_values[ACTUATORS_PWM_NB];
//already declared in actuators.h
//extern void actuators_init(void);
-#include "actuators_pwm_arch.h"
#endif /* ACTUATORS_PWM_H */
diff --git a/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm_supervision.c b/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm_supervision.c
new file mode 100644
index 0000000000..9debad1cfc
--- /dev/null
+++ b/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm_supervision.c
@@ -0,0 +1,63 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010 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.
+ */
+
+#include "firmwares/rotorcraft/actuators.h"
+#include "actuators_pwm_supervision.h"
+#include "booz/booz2_commands.h"
+#include "subsystems/radio_control.h"
+
+/* let's start butchery now and use the actuators_pwm arch functions */
+#include "firmwares/rotorcraft/actuators/actuators_pwm.h"
+
+#include "generated/airframe.h"
+
+int32_t actuators_pwm_values[ACTUATORS_PWM_NB];
+
+void actuators_init(void)
+{
+ supervision_init();
+ actuators_pwm_arch_init();
+}
+
+#define PWM_GAIN_SCALE 2
+#define PWM_OFF 1000
+
+void actuators_set(bool_t motors_on) {
+ booz2_commands[COMMAND_PITCH] = booz2_commands[COMMAND_PITCH] * PWM_GAIN_SCALE;
+ booz2_commands[COMMAND_ROLL] = booz2_commands[COMMAND_ROLL] * PWM_GAIN_SCALE;
+ booz2_commands[COMMAND_YAW] = booz2_commands[COMMAND_YAW] * PWM_GAIN_SCALE;
+ booz2_commands[COMMAND_THRUST] = (booz2_commands[COMMAND_THRUST] * ((SUPERVISION_MAX_MOTOR - SUPERVISION_MIN_MOTOR) / 200)) + SUPERVISION_MIN_MOTOR;
+
+ supervision_run(motors_on, FALSE, booz2_commands);
+
+ if (motors_on) {
+ for (int i = 0; i < SUPERVISION_NB_MOTOR; i++)
+ actuators_pwm_values[i] = supervision.commands[i];
+ } else {
+ for (int i = 0; i < ACTUATORS_PWM_NB; i++)
+ actuators_pwm_values[i] = PWM_OFF;
+ }
+ actuators_pwm_commit();
+
+}
+
diff --git a/sw/airborne/firmwares/rotorcraft/battery.c b/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm_supervision.h
similarity index 75%
rename from sw/airborne/firmwares/rotorcraft/battery.c
rename to sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm_supervision.h
index 357cc448d2..cf10740cc2 100644
--- a/sw/airborne/firmwares/rotorcraft/battery.c
+++ b/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm_supervision.h
@@ -1,7 +1,7 @@
/*
- * $Id$
+ * $Id: actuators_pwm_supervision$
*
- * Copyright (C) 2008-2009 Antoine Drouin
+ * Copyright (C) 2010 The Paparazzi Team
*
* This file is part of paparazzi.
*
@@ -19,12 +19,12 @@
* 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 "firmwares/rotorcraft/battery.h"
+#ifndef ACTUATORS_PWM_SUPERVISION_H
+#define ACTUATORS_PWM_SUPERVISION_H
-uint8_t battery_voltage;
+#include "firmwares/rotorcraft/actuators/supervision.h"
-void battery_init(void) {
- battery_voltage = 0;
-}
+#endif /* ACTUATORS_PWM_SUPERVISION_H */
diff --git a/sw/airborne/firmwares/rotorcraft/actuators/arch/stm32/actuators_pwm_arch.c b/sw/airborne/firmwares/rotorcraft/actuators/arch/stm32/actuators_pwm_arch.c
index f412414b9e..c31ef54232 100644
--- a/sw/airborne/firmwares/rotorcraft/actuators/arch/stm32/actuators_pwm_arch.c
+++ b/sw/airborne/firmwares/rotorcraft/actuators/arch/stm32/actuators_pwm_arch.c
@@ -36,13 +36,35 @@
#define SERVO_HZ 40
#endif
+#ifndef PWM_5AND6_TIMER
+#define PWM_5AND6_TIMER TIM4
+#define PWM_5AND6_RCC RCC_APB1Periph_TIM4
+#define PWM_5AND6_GPIO GPIOB
+#define PWM5_OC 3
+#define PWM6_OC 4
+#define PWM5_Pin GPIO_Pin_8
+#define PWM6_Pin GPIO_Pin_9
+#endif
+
+#define _TIM_OC_INIT(n) TIM_OC##n##Init
+#define TIM_OC_INIT(n) _TIM_OC_INIT(n)
+
+#define _TIM_OC_PRELOADCONFIG(n) TIM_OC##n##PreloadConfig
+#define TIM_OC_PRELOADCONFIG(n) _TIM_OC_PRELOADCONFIG(n)
+
+#define _TIM_SETCOMPARE(n) TIM_SetCompare##n
+#define TIM_SETCOMPARE(n) _TIM_SETCOMPARE(n)
+
void actuators_pwm_arch_init(void) {
/* TIM3 and TIM4 clock enable */
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
+ RCC_APB1PeriphClockCmd(PWM_5AND6_RCC, ENABLE);
+#ifdef USE_SERVOS_7AND8
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
+#endif
- /* GPIOB and GPIOC clock enable */
+ /* GPIO A,B and C clock enable */
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOC |
RCC_APB2Periph_AFIO, ENABLE);
/* GPIO C */
@@ -55,10 +77,15 @@ void actuators_pwm_arch_init(void) {
/* need to remate alternate function, pins 37, 38, 39, 40 on LQFP64 */
GPIO_PinRemapConfig(GPIO_FullRemap_TIM3, ENABLE);
- /* GPIO B */
+ /* PWM 5/6 GPIO */
/* PB8=servo5 PB9=servo6 */
- GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8 | GPIO_Pin_9;
+ GPIO_InitStructure.GPIO_Pin = PWM5_Pin | PWM6_Pin;
+ GPIO_Init(PWM_5AND6_GPIO, &GPIO_InitStructure);
+
+#ifdef USE_SERVOS_7AND8
+ GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
GPIO_Init(GPIOB, &GPIO_InitStructure);
+#endif
/* Time base configuration */
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
@@ -68,7 +95,10 @@ void actuators_pwm_arch_init(void) {
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
+ TIM_TimeBaseInit(PWM_5AND6_TIMER, &TIM_TimeBaseStructure);
+#ifdef USE_SERVOS_7AND8
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
+#endif
/* PWM1 Mode configuration: All Channels */
TIM_OCInitTypeDef TIM_OCInitStructure;
@@ -81,35 +111,50 @@ void actuators_pwm_arch_init(void) {
TIM_OC1Init(TIM3, &TIM_OCInitStructure);
TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable);
- /* PWM1 Mode configuration: TIM3 Channel2 */
+ /* PWM2 Mode configuration: TIM3 Channel2 */
TIM_OC2Init(TIM3, &TIM_OCInitStructure);
TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable);
- /* PWM1 Mode configuration: TIM3 Channel3 */
+ /* PWM3 Mode configuration: TIM3 Channel3 */
TIM_OC3Init(TIM3, &TIM_OCInitStructure);
TIM_OC3PreloadConfig(TIM3, TIM_OCPreload_Enable);
- /* PWM1 Mode configuration: TIM3 Channel4 */
+ /* PWM4 Mode configuration: TIM3 Channel4 */
TIM_OC4Init(TIM3, &TIM_OCInitStructure);
TIM_OC4PreloadConfig(TIM3, TIM_OCPreload_Enable);
- /* PWM1 Mode configuration: TIM4 Channel3 */
- TIM_OC3Init(TIM4, &TIM_OCInitStructure);
- TIM_OC3PreloadConfig(TIM4, TIM_OCPreload_Enable);
+ /* PWM5 Mode configuration: TIM4 Channel3 */
+ TIM_OC_INIT(PWM5_OC) (PWM_5AND6_TIMER, &TIM_OCInitStructure);
+ TIM_OC_PRELOADCONFIG(PWM5_OC)(PWM_5AND6_TIMER, TIM_OCPreload_Enable);
- /* PWM1 Mode configuration: TIM4 Channel4 */
- TIM_OC4Init(TIM4, &TIM_OCInitStructure);
- TIM_OC4PreloadConfig(TIM4, TIM_OCPreload_Enable);
+ /* PWM6 Mode configuration: TIM4 Channel4 */
+ TIM_OC_INIT(PWM6_OC)(PWM_5AND6_TIMER, &TIM_OCInitStructure);
+ TIM_OC_PRELOADCONFIG(PWM6_OC)(PWM_5AND6_TIMER, TIM_OCPreload_Enable);
- /* TIM3 enable */
- TIM_ARRPreloadConfig(TIM3, ENABLE);
- TIM_CtrlPWMOutputs(TIM3, ENABLE);
- TIM_Cmd(TIM3, ENABLE);
+#ifdef USE_SERVOS_7AND8
+ /* PWM7 Mode configuration: TIM4 Channel3 */
+ TIM_OC1Init(TIM4, &TIM_OCInitStructure);
+ TIM_OC1PreloadConfig(TIM4, TIM_OCPreload_Enable);
+
+ /* PWM8 Mode configuration: TIM4 Channel4 */
+ TIM_OC2Init(TIM4, &TIM_OCInitStructure);
+ TIM_OC2PreloadConfig(TIM4, TIM_OCPreload_Enable);
/* TIM4 enable */
TIM_ARRPreloadConfig(TIM4, ENABLE);
TIM_CtrlPWMOutputs(TIM4, ENABLE);
TIM_Cmd(TIM4, ENABLE);
+#endif
+
+ /* PWM1-4 enable */
+ TIM_ARRPreloadConfig(TIM3, ENABLE);
+ TIM_CtrlPWMOutputs(TIM3, ENABLE);
+ TIM_Cmd(TIM3, ENABLE);
+
+ /* PWM5/6 enable */
+ TIM_ARRPreloadConfig(PWM_5AND6_TIMER, ENABLE);
+ TIM_CtrlPWMOutputs(PWM_5AND6_TIMER, ENABLE);
+ TIM_Cmd(PWM_5AND6_TIMER, ENABLE);
}
@@ -119,6 +164,12 @@ void actuators_pwm_commit(void) {
TIM_SetCompare2(TIM3, actuators_pwm_values[1]);
TIM_SetCompare3(TIM3, actuators_pwm_values[2]);
TIM_SetCompare4(TIM3, actuators_pwm_values[3]);
- TIM_SetCompare3(TIM4, actuators_pwm_values[4]);
- TIM_SetCompare4(TIM4, actuators_pwm_values[5]);
+
+ TIM_SETCOMPARE(PWM5_OC)(PWM_5AND6_TIMER, actuators_pwm_values[4]);
+ TIM_SETCOMPARE(PWM6_OC)(PWM_5AND6_TIMER, actuators_pwm_values[5]);
+
+#ifdef USE_SERVOS_7AND8
+ TIM_SetCompare1(TIM4, actuators_pwm_values[6]);
+ TIM_SetCompare2(TIM4, actuators_pwm_values[7]);
+#endif
}
diff --git a/sw/airborne/firmwares/rotorcraft/actuators/arch/stm32/actuators_pwm_arch.h b/sw/airborne/firmwares/rotorcraft/actuators/arch/stm32/actuators_pwm_arch.h
index e1482b22b1..7dbcb8f611 100644
--- a/sw/airborne/firmwares/rotorcraft/actuators/arch/stm32/actuators_pwm_arch.h
+++ b/sw/airborne/firmwares/rotorcraft/actuators/arch/stm32/actuators_pwm_arch.h
@@ -29,6 +29,12 @@
#ifndef ACTUATORS_PWM_ARCH_H
#define ACTUATORS_PWM_ARCH_H
+#ifdef USE_SERVOS_7AND8
+#define ACTUATORS_PWM_NB 8
+#else
+#define ACTUATORS_PWM_NB 6
+#endif
+
extern void actuators_pwm_arch_init(void);
extern void actuators_pwm_commit(void);
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c
index c3026d4a4b..405bde903e 100644
--- a/sw/airborne/firmwares/rotorcraft/autopilot.c
+++ b/sw/airborne/firmwares/rotorcraft/autopilot.c
@@ -65,7 +65,9 @@ void autopilot_init(void) {
autopilot_flight_time = 0;
autopilot_rc = TRUE;
autopilot_power_switch = FALSE;
+#ifdef POWER_SWITCH_LED
LED_ON(POWER_SWITCH_LED); // POWER OFF
+#endif
}
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.h b/sw/airborne/firmwares/rotorcraft/autopilot.h
index 10defb26d0..9c611a7316 100644
--- a/sw/airborne/firmwares/rotorcraft/autopilot.h
+++ b/sw/airborne/firmwares/rotorcraft/autopilot.h
@@ -95,11 +95,17 @@ extern uint16_t autopilot_flight_time;
else autopilot_motors_on = TRUE; \
}
+#ifdef POWER_SWITCH_LED
#define autopilot_SetPowerSwitch(_v) { \
autopilot_power_switch = _v; \
if (_v) { LED_OFF(POWER_SWITCH_LED); } \
else { LED_ON(POWER_SWITCH_LED); } \
}
+#else
+#define autopilot_SetPowerSwitch(_v) { \
+ autopilot_power_switch = _v; \
+ }
+#endif
#ifndef TRESHOLD_GROUND_DETECT
#define TRESHOLD_GROUND_DETECT ACCEL_BFP_OF_REAL(15.)
diff --git a/sw/airborne/firmwares/rotorcraft/battery.h b/sw/airborne/firmwares/rotorcraft/battery.h
deleted file mode 100644
index 41aa171692..0000000000
--- a/sw/airborne/firmwares/rotorcraft/battery.h
+++ /dev/null
@@ -1,43 +0,0 @@
-/*
- * $Id$
- *
- * Copyright (C) 2008-2009 Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- */
-
-#ifndef BATTERY_H
-#define BATTERY_H
-
-#include "std.h"
-
-#include "generated/airframe.h"
-
-/* decivolts */
-extern uint8_t battery_voltage;
-
-static inline void BatteryISRHandler(uint16_t _val) {
- uint32_t cal_v = (uint32_t)(_val) * BATTERY_SENS_NUM / BATTERY_SENS_DEN;
- uint32_t sum = (uint32_t)battery_voltage + cal_v;
- battery_voltage = (uint8_t)(sum/2);
-}
-
-
-extern void battery_init(void);
-
-#endif /* BATTERY_H */
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
index dff5047344..8dbdf59828 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
@@ -37,6 +37,14 @@
#include "generated/airframe.h"
+/* In case Asctec controllers are used without supervision */
+#ifndef SUPERVISION_MIN_MOTOR
+#define SUPERVISION_MIN_MOTOR 1
+#endif
+#ifndef SUPERVISION_MAX_MOTOR
+#define SUPERVISION_MAX_MOTOR 200
+#endif
+
uint8_t guidance_v_mode;
int32_t guidance_v_ff_cmd;
int32_t guidance_v_fb_cmd;
@@ -153,8 +161,9 @@ void guidance_v_run(bool_t in_flight) {
// AKA SUPERVISION and co
if (in_flight) {
// we should use something after the supervision!!! fuck!!!
- int32_t cmd_hack = Chop(stabilization_cmd[COMMAND_THRUST], 1, 200);
+ int32_t cmd_hack = Chop(stabilization_cmd[COMMAND_THRUST], SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR);
gv_adapt_run(ins_ltp_accel.z, cmd_hack);
+ //gv_adapt_run(ins_ltp_accel.z, cmd_hack, guidance_v_zd_ref);
}
else {
// reset vertical filter until takeoff
diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c
index ad0bf0e3c0..1a960eda0c 100644
--- a/sw/airborne/firmwares/rotorcraft/main.c
+++ b/sw/airborne/firmwares/rotorcraft/main.c
@@ -32,6 +32,7 @@
#include "downlink.h"
#include "firmwares/rotorcraft/telemetry.h"
#include "datalink.h"
+#include "subsystems/settings.h"
#include "xbee.h"
#include "booz2_commands.h"
@@ -41,13 +42,11 @@
#include "subsystems/imu.h"
#include "booz_gps.h"
-#include "booz/booz2_analog.h"
#include "subsystems/sensors/baro.h"
#include "baro_board.h"
-#include "firmwares/rotorcraft/battery.h"
+#include "subsystems/electrical.h"
-// #include "booz_fms.h" // FIXME
#include "firmwares/rotorcraft/autopilot.h"
#include "firmwares/rotorcraft/stabilization.h"
@@ -98,6 +97,7 @@ STATIC_INLINE void main_init( void ) {
mcu_init();
sys_time_init();
+ electrical_init();
actuators_init();
radio_control_init();
@@ -106,13 +106,8 @@ STATIC_INLINE void main_init( void ) {
xbee_init();
#endif
- booz2_analog_init();
baro_init();
-
- battery_init();
imu_init();
-
- // booz_fms_init(); // FIXME
autopilot_init();
nav_init();
guidance_h_init();
@@ -130,6 +125,8 @@ STATIC_INLINE void main_init( void ) {
modules_init();
+ settings_init();
+
mcu_int_enable();
}
@@ -156,7 +153,7 @@ STATIC_INLINE void main_periodic( void ) {
/* booz_fms_periodic(); FIXME */ \
}, \
{ \
- /*BoozControlSurfacesSetFromCommands();*/ \
+ electrical_periodic(); \
}, \
{ \
LED_PERIODIC(); \
@@ -172,16 +169,12 @@ STATIC_INLINE void main_periodic( void ) {
} );
#ifdef USE_GPS
- if (radio_control.status != RC_OK && \
+ if (radio_control.status != RC_OK && \
autopilot_mode == AP_MODE_NAV && GpsIsLost()) \
autopilot_set_mode(AP_MODE_FAILSAFE); \
booz_gps_periodic();
#endif
-#ifdef USE_EXTRA_ADC
- booz2_analog_periodic();
-#endif
-
modules_periodic_task();
if (autopilot_in_flight) {
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
index 2680feef43..36f6dd9df7 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
@@ -200,7 +200,7 @@ void stabilization_attitude_run(bool_t enable_integrator) {
scaled_att_err.qy = att_err.qy / IERROR_SCALE;
scaled_att_err.qz = att_err.qz / IERROR_SCALE;
FLOAT_QUAT_COMP_INV(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err);
- FLOAT_QUAT_NORMALISE(new_sum_err);
+ FLOAT_QUAT_NORMALIZE(new_sum_err);
FLOAT_QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err);
FLOAT_EULERS_OF_QUAT(stabilization_att_sum_err_eulers, stabilization_att_sum_err_quat);
} else {
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c
index 7799d9995b..c6478d5b40 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c
@@ -117,7 +117,7 @@ void stabilization_attitude_ref_update() {
FLOAT_QUAT_DERIVATIVE(qdot, stab_att_ref_rate, stab_att_ref_quat);
QUAT_SMUL(qdot, qdot, DT_UPDATE);
QUAT_ADD(stab_att_ref_quat, qdot);
- FLOAT_QUAT_NORMALISE(stab_att_ref_quat);
+ FLOAT_QUAT_NORMALIZE(stab_att_ref_quat);
/* integrate reference rotational speeds */
struct FloatRates delta_rate;
diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h
index 1cf5801b80..65a755cbb6 100644
--- a/sw/airborne/firmwares/rotorcraft/telemetry.h
+++ b/sw/airborne/firmwares/rotorcraft/telemetry.h
@@ -45,7 +45,7 @@
#define PERIODIC_SEND_ALIVE(_chan) DOWNLINK_SEND_ALIVE(_chan, 16, MD5SUM)
-#include "firmwares/rotorcraft/battery.h"
+#include "subsystems/electrical.h"
#include "subsystems/imu.h"
#include "booz_gps.h"
#include "subsystems/ins.h"
@@ -56,43 +56,43 @@
extern uint8_t telemetry_mode_Main_DefaultChannel;
#ifdef USE_GPS
-#define PERIODIC_SEND_ROTORCRAFT_STATUS(_chan) { \
- uint32_t imu_nb_err = 0; \
+#define PERIODIC_SEND_ROTORCRAFT_STATUS(_chan) { \
+ uint32_t imu_nb_err = 0; \
uint8_t _twi_blmc_nb_err = 0; \
- DOWNLINK_SEND_ROTORCRAFT_STATUS(_chan, \
- &imu_nb_err, \
- &_twi_blmc_nb_err, \
- &radio_control.status, \
- &radio_control.frame_rate, \
- &booz_gps_state.fix, \
- &autopilot_mode, \
- &autopilot_in_flight, \
- &autopilot_motors_on, \
- &guidance_h_mode, \
- &guidance_v_mode, \
- &battery_voltage, \
- &cpu_time_sec \
- ); \
+ DOWNLINK_SEND_ROTORCRAFT_STATUS(_chan, \
+ &imu_nb_err, \
+ &_twi_blmc_nb_err, \
+ &radio_control.status, \
+ &radio_control.frame_rate, \
+ &booz_gps_state.fix, \
+ &autopilot_mode, \
+ &autopilot_in_flight, \
+ &autopilot_motors_on, \
+ &guidance_h_mode, \
+ &guidance_v_mode, \
+ &electrical.vsupply, \
+ &cpu_time_sec \
+ ); \
}
#else /* !USE_GPS */
-#define PERIODIC_SEND_ROTORCRAFT_STATUS(_chan) { \
- uint32_t imu_nb_err = 0; \
+#define PERIODIC_SEND_ROTORCRAFT_STATUS(_chan) { \
+ uint32_t imu_nb_err = 0; \
uint8_t twi_blmc_nb_err = 0; \
uint8_t fix = BOOZ2_GPS_FIX_NONE; \
- DOWNLINK_SEND_ROTORCRAFT_STATUS(_chan, \
- &imu_nb_err, \
- &twi_blmc_nb_err, \
- &radio_control.status, \
- &radio_control.frame_rate, \
- &fix, \
- &autopilot_mode, \
- &autopilot_in_flight, \
- &autopilot_motors_on, \
- &guidance_h_mode, \
- &guidance_v_mode, \
- &battery_voltage, \
- &cpu_time_sec \
- ); \
+ DOWNLINK_SEND_ROTORCRAFT_STATUS(_chan, \
+ &imu_nb_err, \
+ &twi_blmc_nb_err, \
+ &radio_control.status, \
+ &radio_control.frame_rate, \
+ &fix, \
+ &autopilot_mode, \
+ &autopilot_in_flight, \
+ &autopilot_motors_on, \
+ &guidance_h_mode, \
+ &guidance_v_mode, \
+ &electrical.vsupply, \
+ &cpu_time_sec \
+ ); \
}
#endif /* USE_GPS */
@@ -130,22 +130,22 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
#define PERIODIC_SEND_PPM(_chan) {}
#endif
-#define PERIODIC_SEND_BOOZ2_GYRO(_chan) { \
- DOWNLINK_SEND_BOOZ2_GYRO(_chan, \
+#define PERIODIC_SEND_IMU_GYRO_SCALED(_chan) { \
+ DOWNLINK_SEND_IMU_GYRO_SCALED(_chan, \
&imu.gyro.p, \
&imu.gyro.q, \
&imu.gyro.r); \
}
-#define PERIODIC_SEND_BOOZ2_ACCEL(_chan) { \
- DOWNLINK_SEND_BOOZ2_ACCEL(_chan, \
+#define PERIODIC_SEND_IMU_ACCEL_SCALED(_chan) { \
+ DOWNLINK_SEND_IMU_ACCEL_SCALED(_chan, \
&imu.accel.x, \
&imu.accel.y, \
&imu.accel.z); \
}
-#define PERIODIC_SEND_BOOZ2_MAG(_chan) { \
- DOWNLINK_SEND_BOOZ2_MAG(_chan, \
+#define PERIODIC_SEND_IMU_MAG_SCALED(_chan) { \
+ DOWNLINK_SEND_IMU_MAG_SCALED(_chan, \
&imu.mag.x, \
&imu.mag.y, \
&imu.mag.z); \
diff --git a/sw/airborne/fms/fms_spi_autopilot_msg.c b/sw/airborne/fms/fms_spi_autopilot_msg.c
index 33c0a53ee1..8df37b8ba3 100644
--- a/sw/airborne/fms/fms_spi_autopilot_msg.c
+++ b/sw/airborne/fms/fms_spi_autopilot_msg.c
@@ -106,7 +106,7 @@ int spi_ap_link_init()
FLOAT_QUAT_OF_AXIS_ANGLE(imuFloat.body_to_imu_quat, x_axis, QUAT_SETPOINT_HOVER_PITCH);
#endif
- FLOAT_QUAT_NORMALISE(imuFloat.body_to_imu_quat);
+ FLOAT_QUAT_NORMALIZE(imuFloat.body_to_imu_quat);
FLOAT_EULERS_OF_QUAT(imuFloat.body_to_imu_eulers, imuFloat.body_to_imu_quat);
FLOAT_RMAT_OF_QUAT(imuFloat.body_to_imu_rmat, imuFloat.body_to_imu_quat);
diff --git a/sw/airborne/gps_ubx.c b/sw/airborne/gps_ubx.c
index 581a3cb4ce..d2254f69a8 100644
--- a/sw/airborne/gps_ubx.c
+++ b/sw/airborne/gps_ubx.c
@@ -257,7 +257,7 @@ void parse_gps_msg( void ) {
gps_status_flags = UBX_NAV_STATUS_Flags(ubx_msg_buf);
gps_sol_flags = UBX_NAV_SOL_Flags(ubx_msg_buf);
#ifdef GPS_USE_LATLONG
- /* Computes from (lat, long) in the referenced UTM zone */
+ /* Computes from (lat, long) in the referenced UTM zone */
} else if (ubx_id == UBX_NAV_POSLLH_ID) {
gps_lat = UBX_NAV_POSLLH_LAT(ubx_msg_buf);
gps_lon = UBX_NAV_POSLLH_LON(ubx_msg_buf);
@@ -275,7 +275,7 @@ void parse_gps_msg( void ) {
gps_utm_north = UBX_NAV_POSUTM_NORTH(ubx_msg_buf);
uint8_t hem = UBX_NAV_POSUTM_HEM(ubx_msg_buf);
if (hem == UTM_HEM_SOUTH)
- gps_utm_north -= 1000000000; /* Subtract false northing: -10000km */
+ gps_utm_north -= 1000000000; /* Subtract false northing: -10000km */
gps_alt = UBX_NAV_POSUTM_ALT(ubx_msg_buf);
gps_utm_zone = UBX_NAV_POSUTM_ZONE(ubx_msg_buf);
#endif
@@ -306,12 +306,12 @@ void parse_gps_msg( void ) {
gps_nb_channels = Min(UBX_NAV_SVINFO_NCH(ubx_msg_buf), GPS_NB_CHANNELS);
uint8_t i;
for(i = 0; i < gps_nb_channels; i++) {
- gps_svinfos[i].svid = UBX_NAV_SVINFO_SVID(ubx_msg_buf, i);
- gps_svinfos[i].flags = UBX_NAV_SVINFO_Flags(ubx_msg_buf, i);
- gps_svinfos[i].qi = UBX_NAV_SVINFO_QI(ubx_msg_buf, i);
- gps_svinfos[i].cno = UBX_NAV_SVINFO_CNO(ubx_msg_buf, i);
- gps_svinfos[i].elev = UBX_NAV_SVINFO_Elev(ubx_msg_buf, i);
- gps_svinfos[i].azim = UBX_NAV_SVINFO_Azim(ubx_msg_buf, i);
+ gps_svinfos[i].svid = UBX_NAV_SVINFO_SVID(ubx_msg_buf, i);
+ gps_svinfos[i].flags = UBX_NAV_SVINFO_Flags(ubx_msg_buf, i);
+ gps_svinfos[i].qi = UBX_NAV_SVINFO_QI(ubx_msg_buf, i);
+ gps_svinfos[i].cno = UBX_NAV_SVINFO_CNO(ubx_msg_buf, i);
+ gps_svinfos[i].elev = UBX_NAV_SVINFO_Elev(ubx_msg_buf, i);
+ gps_svinfos[i].azim = UBX_NAV_SVINFO_Azim(ubx_msg_buf, i);
}
}
}
diff --git a/sw/airborne/lisa/lisa_analog_plug.c b/sw/airborne/lisa/lisa_analog_plug.c
deleted file mode 100644
index 3e49d689d9..0000000000
--- a/sw/airborne/lisa/lisa_analog_plug.c
+++ /dev/null
@@ -1,10 +0,0 @@
-#include "std.h"
-
-uint8_t battery_voltage;
-
-void booz2_analog_init(void);
-void battery_init(void);
-
-void booz2_analog_init(void) {}
-void battery_init(void) {}
-
diff --git a/sw/airborne/lisa/test/lisa_test_ms2001.c b/sw/airborne/lisa/test/lisa_test_ms2001.c
deleted file mode 100644
index 7fe419587c..0000000000
--- a/sw/airborne/lisa/test/lisa_test_ms2001.c
+++ /dev/null
@@ -1,123 +0,0 @@
-/*
- * $Id$
- *
- * Copyright (C) 2010 Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- */
-
-#include
-#include
-#include
-#include
-#include
-#include
-
-#include BOARD_CONFIG
-#include "mcu.h"
-#include "sys_time.h"
-#include "downlink.h"
-#include "peripherals/ms2001.h"
-
-static inline void main_init( void );
-static inline void main_periodic_task( void );
-static inline void main_event_task( void );
-
-static inline void main_spi2_init(void);
-
-int main(void) {
- main_init();
-
- while(1) {
- if (sys_time_periodic())
- main_periodic_task();
- main_event_task();
- }
-
- return 0;
-}
-
-
-static inline void main_init( void ) {
- mcu_init();
- sys_time_init();
- ms2001_init();
- main_spi2_init();
-}
-
-static inline void main_periodic_task( void ) {
- RunOnceEvery(10,
- {
- DOWNLINK_SEND_BOOT(DefaultChannel, &cpu_time_sec);
- LED_PERIODIC();
- });
-
- switch(ms2001_status) {
- case MS2001_IDLE:
- Ms2001SendReq();
- break;
- case MS2001_WAITING_EOC:
- if (Ms2001HasEOC()) {
- Ms2001ReadRes();
- }
- break;
- }
-}
-
-static inline void main_event_task( void ) {
- if (ms2001_status == MS2001_DATA_AVAILABLE) {
- RunOnceEvery(10, {
- DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,
- &ms2001_values[0],
- &ms2001_values[1],
- &ms2001_values[2]);
- });
- ms2001_status = MS2001_IDLE;
- }
-}
-
-static inline void main_spi2_init( void ) {
-
- /* set max1168 slave select as output and assert it ( on PB12) */
- GPIOB->BSRR = GPIO_Pin_12;
- RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
- GPIO_InitTypeDef GPIO_InitStructure;
- GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_Init(GPIOB, &GPIO_InitStructure);
-
- /* Enable SPI2 Periph clock -------------------------------------------------*/
- RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2, ENABLE);
-
- /* Configure GPIOs: SCK, MISO and MOSI --------------------------------*/
- GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
- GPIO_Init(GPIOB, &GPIO_InitStructure);
-
- RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO , ENABLE);
-
-
- /* Enable SPI_2 DMA clock ---------------------------------------------------*/
- RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
-
-
-
-}
-
diff --git a/sw/airborne/lisa/test/lisa_test_ms2100.c b/sw/airborne/lisa/test/lisa_test_ms2100.c
index aabd4aa9ed..1bf869b3fa 100644
--- a/sw/airborne/lisa/test/lisa_test_ms2100.c
+++ b/sw/airborne/lisa/test/lisa_test_ms2100.c
@@ -1,7 +1,7 @@
/*
* $Id$
*
- * Copyright (C) 2009 Antoine Drouin
+ * Copyright (C) 2010 Antoine Drouin
*
* This file is part of paparazzi.
*
@@ -56,46 +56,51 @@ int main(void) {
static inline void main_init( void ) {
mcu_init();
sys_time_init();
- max1168_init();
+ ms2100_init();
main_spi2_init();
}
static inline void main_periodic_task( void ) {
- // LED_TOGGLE(6);
- max1168_read();
RunOnceEvery(10,
{
DOWNLINK_SEND_BOOT(DefaultChannel, &cpu_time_sec);
LED_PERIODIC();
});
-}
-
-static inline void main_event_task( void ) {
- if (max1168_status == STA_MAX1168_DATA_AVAILABLE) {
- RunOnceEvery(10, {
- DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, &max1168_values[0], &max1168_values[1], &max1168_values[2]);
- DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, &max1168_values[3], &max1168_values[4], &max1168_values[6]);
- // DOWNLINK_SEND_BOOT(DefaultChannel, &max1168_values[7]); });
- });
- max1168_status = STA_MAX1168_IDLE;
+ switch(ms2100_status) {
+ case MS2100_IDLE:
+ Ms2001SendReq();
+ break;
+ case MS2100_WAITING_EOC:
+ if (Ms2001HasEOC()) {
+ Ms2001ReadRes();
+ }
+ break;
}
}
+static inline void main_event_task( void ) {
+ if (ms2100_status == MS2100_DATA_AVAILABLE) {
+ RunOnceEvery(10, {
+ DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,
+ &ms2100_values[0],
+ &ms2100_values[1],
+ &ms2100_values[2]);
+ });
+ ms2100_status = MS2100_IDLE;
+ }
+}
static inline void main_spi2_init( void ) {
- /* set mag ss as output and assert it (on PC12) ------------------------------*/
- /* set mag reset as output and assert it (on PC13) ------------------------------*/
- GPIOC->BSRR = GPIO_Pin_12;
- GPIOC->BSRR = GPIO_Pin_13;
-
- RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);
+ /* set max1168 slave select as output and assert it ( on PB12) */
+ GPIOB->BSRR = GPIO_Pin_12;
+ RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
GPIO_InitTypeDef GPIO_InitStructure;
- GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12 | GPIO_Pin_13;
+ GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_Init(GPIOC, &GPIO_InitStructure);
+ GPIO_Init(GPIOB, &GPIO_InitStructure);
/* Enable SPI2 Periph clock -------------------------------------------------*/
RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2, ENABLE);
@@ -108,24 +113,11 @@ static inline void main_spi2_init( void ) {
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO , ENABLE);
- /* SPI Master configuration --------------------------------------------------*/
- SPI_InitTypeDef SPI_InitStructure;
- SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
- SPI_InitStructure.SPI_Mode = SPI_Mode_Master;
- SPI_InitStructure.SPI_DataSize = SPI_DataSize_16b;
- SPI_InitStructure.SPI_CPOL = SPI_CPOL_Low;
- SPI_InitStructure.SPI_CPHA = SPI_CPHA_1Edge;
- SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;
- SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8;
- SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;
- SPI_InitStructure.SPI_CRCPolynomial = 7;
- SPI_Init(SPI2, &SPI_InitStructure);
-
- /* Enable SPI */
- SPI_Cmd(SPI2, ENABLE);
/* Enable SPI_2 DMA clock ---------------------------------------------------*/
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
+
+
}
diff --git a/sw/airborne/lisa/test_led.c b/sw/airborne/lisa/test_led.c
index 0052006f56..f3d5d1269d 100644
--- a/sw/airborne/lisa/test_led.c
+++ b/sw/airborne/lisa/test_led.c
@@ -29,12 +29,62 @@
#include "led.h"
void Delay(__IO uint32_t nCount);
+void led_on(int i);
+void led_off(int i);
+#ifdef BOARD_LISA_L
#define LED_PROGRAM_SIZE 26
const int LED_PROG_ON[LED_PROGRAM_SIZE] = { 3, 5, 7, 1, -1, -1, -1, -1, 2, 4, 6, 0, 3, 5, 7, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 };
const int LED_PROG_OFF[LED_PROGRAM_SIZE] = {-1, -1, -1, -1, 3, 5, 7, 1, -1, -1, -1, -1, -1, -1, -1, -1, 3, 5, 7, 1, 2, 4, 6, 0, -1, -1 };
+#endif
+#ifdef BOARD_LISA_M
+#define LED_PROGRAM_SIZE 10
+
+const int LED_PROG_ON[LED_PROGRAM_SIZE] = { 1, 2, -1, -1, -1, 2, 1, -1, -1, -1 };
+const int LED_PROG_OFF[LED_PROGRAM_SIZE] = { -1, -1, 1, 2, -1, -1, -1, 2, 1, -1 };
+#endif
+
+void led_on(int i) {
+#ifdef BOARD_LISA_L
+ LED_ON(i);
+#endif
+
+#ifdef BOARD_LISA_M
+ switch (i) {
+ case 1:
+ LED_ON(1);
+ break;
+ case 2:
+ LED_ON(2);
+ break;
+ default:
+ /* ignore as we only have 2 led's for now on lisa/m */
+ break;
+ }
+#endif
+}
+
+void led_off(int i) {
+#ifdef BOARD_LISA_L
+ LED_OFF(i);
+#endif
+
+#ifdef BOARD_LISA_M
+ switch (i) {
+ case 1:
+ LED_OFF(1);
+ break;
+ case 2:
+ LED_OFF(2);
+ break;
+ default:
+ /* ignore as we only have 2 led's for now on lisa/m */
+ break;
+ }
+#endif
+}
int main(void) {
int i = 0;
@@ -43,11 +93,11 @@ int main(void) {
for (i=0; i< LED_PROGRAM_SIZE; i++)
{
if (LED_PROG_ON[i] >= 0)
- LED_ON(LED_PROG_ON[i]);
+ led_on(LED_PROG_ON[i]);
LED_PERIODIC();
Delay(2000000);
if (LED_PROG_OFF[i] >= 0)
- LED_OFF(LED_PROG_OFF[i]);
+ led_off(LED_PROG_OFF[i]);
}
};
return 0;
diff --git a/sw/airborne/lisa/test_servos.c b/sw/airborne/lisa/test_servos.c
index 533feedc1a..98ae30c1f2 100644
--- a/sw/airborne/lisa/test_servos.c
+++ b/sw/airborne/lisa/test_servos.c
@@ -50,14 +50,9 @@ static inline void main_periodic( void ) {
static float foo = 0.;
foo += 0.0025;
int32_t bar = 1500 + 500. * sin(foo);
- // int32_t bar = 1450;
- // int32_t bar = 1950;
- actuators_pwm_values[0] = bar;
- actuators_pwm_values[1] = bar;
- actuators_pwm_values[2] = bar;
- actuators_pwm_values[3] = bar;
- actuators_pwm_values[4] = bar;
- actuators_pwm_values[5] = bar;
+ for (int i = 0; i < ACTUATORS_PWM_NB; i++) {
+ actuators_pwm_values[i] = bar;
+ }
actuators_pwm_commit();
LED_PERIODIC();
diff --git a/sw/airborne/lisa/test_uart_lisam.c b/sw/airborne/lisa/test_uart_lisam.c
new file mode 100644
index 0000000000..368b31bed5
--- /dev/null
+++ b/sw/airborne/lisa/test_uart_lisam.c
@@ -0,0 +1,100 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2009 Antoine Drouin
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+#include
+#include
+
+#include
+#include
+
+#include BOARD_CONFIG
+#include "mcu.h"
+#include "mcu_periph/uart.h"
+#include "sys_time.h"
+
+static inline void main_init( void );
+static inline void main_periodic( void );
+
+int main(void) {
+
+ main_init();
+
+ while (1) {
+ if (sys_time_periodic())
+ main_periodic();
+ }
+ return 0;
+}
+
+static inline void main_init( void ) {
+ mcu_init();
+ sys_time_init();
+}
+
+static inline void main_periodic( void ) {
+ char ch;
+
+ Uart1Transmit('a');
+ Uart2Transmit('b');
+ Uart3Transmit('c');
+ Uart5Transmit('d');
+
+ LED_OFF(1);
+ LED_OFF(2);
+
+ if (Uart1ChAvailable()) {
+ ch = Uart1Getch();
+ if (ch == 'a') {
+ LED_ON(1);
+ } else {
+ LED_ON(2);
+ }
+ }
+
+ if (Uart2ChAvailable()) {
+ ch = Uart2Getch();
+ if (ch == 'b') {
+ LED_ON(1);
+ } else {
+ LED_ON(2);
+ }
+ }
+
+ if (Uart3ChAvailable()) {
+ ch = Uart3Getch();
+ if (ch == 'c') {
+ LED_ON(1);
+ } else {
+ LED_ON(2);
+ }
+ }
+
+ if (Uart5ChAvailable()) {
+ ch = Uart5Getch();
+ if (ch == 'd') {
+ LED_ON(1);
+ } else {
+ LED_ON(2);
+ }
+ }
+}
diff --git a/sw/airborne/math/pprz_algebra.h b/sw/airborne/math/pprz_algebra.h
index f090c8a189..81b35c7b54 100644
--- a/sw/airborne/math/pprz_algebra.h
+++ b/sw/airborne/math/pprz_algebra.h
@@ -26,6 +26,7 @@
#define PPRZ_ALGEBRA_H
#include /* for FLT_EPSILON */
+#include /* for memcpy */
#define SQUARE(_a) ((_a)*(_a))
@@ -101,7 +102,7 @@
}
/* a = b */
-#define VECT3_COPY(_a, _b) { \
+#define VECT3_COPY(_a, _b) { \
(_a).x = (_b).x; \
(_a).y = (_b).y; \
(_a).z = (_b).z; \
@@ -128,6 +129,13 @@
(_c).z = (_a).z + (_b).z; \
}
+/* a += b*s */
+#define VECT3_ADD_SCALED(_a, _b, _s) { \
+ (_a).x += ((_b).x * (_s)); \
+ (_a).y += ((_b).y * (_s)); \
+ (_a).z += ((_b).z * (_s)); \
+ }
+
/* c = a + _s * b */
#define VECT3_SUM_SCALED(_c, _a, _b, _s) { \
(_c).x = (_a).x + (_s)*(_b).x; \
@@ -186,9 +194,9 @@
/* */
#define VECT3_BOUND_BOX(_v, _v_min, _v_max) { \
- if ((_v).x > (_v_max.x)) (_v).x = (_v_max.x); else if ((_v).x < (_v_min.x)) (_v).x = (_v_min.x); \
- if ((_v).y > (_v_max.y)) (_v).y = (_v_max.y); else if ((_v).y < (_v_min.y)) (_v).y = (_v_min.z); \
- if ((_v).z > (_v_max.y)) (_v).z = (_v_max.z); else if ((_v).z < (_v_min.z)) (_v).z = (_v_min.z); \
+ if ((_v).x > (_v_max).x) (_v).x = (_v_max).x; else if ((_v).x < (_v_min).x) (_v).x = (_v_min).x; \
+ if ((_v).y > (_v_max).y) (_v).y = (_v_max).y; else if ((_v).y < (_v_min).y) (_v).y = (_v_min).z; \
+ if ((_v).z > (_v_max).y) (_v).z = (_v_max).z; else if ((_v).z < (_v_min).z) (_v).z = (_v_min).z; \
}
@@ -204,9 +212,9 @@
}
#define EULERS_ASSIGN(_e, _phi, _theta, _psi) { \
- (_e).phi = _phi; \
- (_e).theta = _theta; \
- (_e).psi = _psi; \
+ (_e).phi = (_phi); \
+ (_e).theta = (_theta); \
+ (_e).psi = (_psi); \
}
/* a += b */
@@ -247,9 +255,9 @@
/* _v = Bound(_v, _min, _max) */
#define EULERS_BOUND_CUBE(_v, _min, _max) { \
- (_v).phi = (_v).phi < _min ? _min : (_v).phi > _max ? _max : (_v).phi; \
- (_v).theta = (_v).theta < _min ? _min : (_v).theta > _max ? _max : (_v).theta; \
- (_v).psi = (_v).psi < _min ? _min : (_v).psi > _max ? _max : (_v).psi; \
+ (_v).phi = (_v).phi < (_min) ? (_min) : (_v).phi > (_max) ? (_max) : (_v).phi; \
+ (_v).theta = (_v).theta < (_min) ? (_min) : (_v).theta > (_max) ? (_max) : (_v).theta; \
+ (_v).psi = (_v).psi < (_min) ? (_min) : (_v).psi > (_max) ? (_max) : (_v).psi; \
}
/*
@@ -313,24 +321,24 @@
}
/* Element wise vector multiplication */
-#define RATES_EWMULT_RSHIFT(c, a, b, _s) { \
- c.p = (a.p * b.p) >> (_s); \
- c.q = (a.q * b.q) >> (_s); \
- c.r = (a.r * b.r) >> (_s); \
+#define RATES_EWMULT_RSHIFT(c, a, b, _s) { \
+ (c).p = ((a).p * (b).p) >> (_s); \
+ (c).q = ((a).q * (b).q) >> (_s); \
+ (c).r = ((a).r * (b).r) >> (_s); \
}
/* _v = Bound(_v, _min, _max) */
#define RATES_BOUND_CUBE(_v, _min, _max) { \
- (_v).p = (_v).p < _min ? _min : (_v).p > _max ? _max : (_v).p; \
- (_v).q = (_v).q < _min ? _min : (_v).q > _max ? _max : (_v).q; \
- (_v).r = (_v).r < _min ? _min : (_v).r > _max ? _max : (_v).r; \
+ (_v).p = (_v).p < (_min) ? (_min) : (_v).p > (_max) ? (_max) : (_v).p; \
+ (_v).q = (_v).q < (_min) ? (_min) : (_v).q > (_max) ? (_max) : (_v).q; \
+ (_v).r = (_v).r < (_min) ? (_min) : (_v).r > (_max) ? (_max) : (_v).r; \
}
#define RATES_BOUND_BOX(_v, _v_min, _v_max) { \
- if ((_v).p > (_v_max.p)) (_v).p = (_v_max.p); else if ((_v).p < (_v_min.p)) (_v).p = (_v_min.p); \
- if ((_v).q > (_v_max.q)) (_v).q = (_v_max.q); else if ((_v).q < (_v_min.q)) (_v).q = (_v_min.q); \
- if ((_v).r > (_v_max.r)) (_v).r = (_v_max.r); else if ((_v).r < (_v_min.r)) (_v).r = (_v_min.r); \
+ if ((_v).p > (_v_max).p) (_v).p = (_v_max).p; else if ((_v).p < (_v_min).p) (_v).p = (_v_min).p; \
+ if ((_v).q > (_v_max).q) (_v).q = (_v_max).q; else if ((_v).q < (_v_min).q) (_v).q = (_v_min).q; \
+ if ((_v).r > (_v_max).r) (_v).r = (_v_max).r; else if ((_v).r < (_v_min).r) (_v).r = (_v_min).r; \
}
@@ -339,7 +347,7 @@
* 3x3 matrices
*/
/* accessor : row and col range from 0 to 2 */
-#define MAT33_ELMT(_m, _row, _col) ((_m).m[_row*3+_col])
+#define MAT33_ELMT(_m, _row, _col) ((_m).m[(_row)*3+(_col)])
#define MAT33_COPY(_mat1,_mat2) { \
MAT33_ELMT((_mat1),0,0) = MAT33_ELMT((_mat2),0,0); \
@@ -427,26 +435,26 @@
(_qc).qz = (_qa).qz - (_qb).qz; \
}
-#define QUAT_COPY(_qo, _qi) { \
+#define QUAT_COPY(_qo, _qi) { \
(_qo).qi = (_qi).qi; \
(_qo).qx = (_qi).qx; \
(_qo).qy = (_qi).qy; \
(_qo).qz = (_qi).qz; \
}
-#define QUAT_EXPLEMENTARY(b,a) { \
- b.qi = -a.qi; \
- b.qx = -a.qx; \
- b.qy = -a.qy; \
- b.qz = -a.qz; \
+#define QUAT_EXPLEMENTARY(b,a) { \
+ (b).qi = -(a).qi; \
+ (b).qx = -(a).qx; \
+ (b).qy = -(a).qy; \
+ (b).qz = -(a).qz; \
}
#define QUAT_SMUL(_qo, _qi, _s) { \
- (_qo).qi = (_qi).qi * _s; \
- (_qo).qx = (_qi).qx * _s; \
- (_qo).qy = (_qi).qy * _s; \
- (_qo).qz = (_qi).qz * _s; \
+ (_qo).qi = (_qi).qi * (_s); \
+ (_qo).qx = (_qi).qx * (_s); \
+ (_qo).qy = (_qi).qy * (_s); \
+ (_qo).qz = (_qi).qz * (_s); \
}
#define QUAT_ADD(_qo, _qi) { \
@@ -500,6 +508,19 @@
RMAT_ELMT((_rmat), 2, 2) * (_vin).z; \
}
+#define RMAT_VECT3_TRANSP_MUL(_vout, _rmat, _vin) { \
+ (_vout).x = RMAT_ELMT((_rmat), 0, 0) * (_vin).x + \
+ RMAT_ELMT((_rmat), 1, 0) * (_vin).y + \
+ RMAT_ELMT((_rmat), 2, 0) * (_vin).z; \
+ (_vout).y = RMAT_ELMT((_rmat), 0, 1) * (_vin).x + \
+ RMAT_ELMT((_rmat), 1, 1) * (_vin).y + \
+ RMAT_ELMT((_rmat), 2, 1) * (_vin).z; \
+ (_vout).z = RMAT_ELMT((_rmat), 0, 2) * (_vin).x + \
+ RMAT_ELMT((_rmat), 1, 2) * (_vin).y + \
+ RMAT_ELMT((_rmat), 2, 2) * (_vin).z; \
+ }
+
+
#define RMAT_COPY(_o, _i) { memcpy(&(_o), &(_i), sizeof(_o));}
@@ -568,27 +589,27 @@
}
#define ACCELS_FLOAT_OF_BFP(_ef, _ei) { \
- (_ef).x = ACCEL_FLOAT_OF_BFP((_ei).x); \
- (_ef).y = ACCEL_FLOAT_OF_BFP((_ei).y); \
- (_ef).z = ACCEL_FLOAT_OF_BFP((_ei).z); \
+ (_ef).x = ACCEL_FLOAT_OF_BFP((_ei).x); \
+ (_ef).y = ACCEL_FLOAT_OF_BFP((_ei).y); \
+ (_ef).z = ACCEL_FLOAT_OF_BFP((_ei).z); \
}
#define ACCELS_BFP_OF_REAL(_ef, _ei) { \
- (_ef).x = ACCEL_BFP_OF_REAL((_ei).x); \
- (_ef).y = ACCEL_BFP_OF_REAL((_ei).y); \
- (_ef).z = ACCEL_BFP_OF_REAL((_ei).z); \
+ (_ef).x = ACCEL_BFP_OF_REAL((_ei).x); \
+ (_ef).y = ACCEL_BFP_OF_REAL((_ei).y); \
+ (_ef).z = ACCEL_BFP_OF_REAL((_ei).z); \
}
#define MAGS_FLOAT_OF_BFP(_ef, _ei) { \
- (_ef).x = MAG_FLOAT_OF_BFP((_ei).x); \
- (_ef).y = MAG_FLOAT_OF_BFP((_ei).y); \
- (_ef).z = MAG_FLOAT_OF_BFP((_ei).z); \
+ (_ef).x = MAG_FLOAT_OF_BFP((_ei).x); \
+ (_ef).y = MAG_FLOAT_OF_BFP((_ei).y); \
+ (_ef).z = MAG_FLOAT_OF_BFP((_ei).z); \
}
#define MAGS_BFP_OF_REAL(_ef, _ei) { \
- (_ef).x = MAG_BFP_OF_REAL((_ei).x); \
- (_ef).y = MAG_BFP_OF_REAL((_ei).y); \
- (_ef).z = MAG_BFP_OF_REAL((_ei).z); \
+ (_ef).x = MAG_BFP_OF_REAL((_ei).x); \
+ (_ef).y = MAG_BFP_OF_REAL((_ei).y); \
+ (_ef).z = MAG_BFP_OF_REAL((_ei).z); \
}
#endif /* PPRZ_ALGEBRA_H */
diff --git a/sw/airborne/math/pprz_algebra_double.h b/sw/airborne/math/pprz_algebra_double.h
index fd70ee2073..d712ae2f5f 100644
--- a/sw/airborne/math/pprz_algebra_double.h
+++ b/sw/airborne/math/pprz_algebra_double.h
@@ -1,3 +1,32 @@
+/*
+ * Copyright (C) 2008-2011 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, see
+ * .
+ *
+ */
+
+/**
+ * @file pprz_algebra_double.h
+ * @brief Paparazzi double precision floating point algebra.
+ *
+ * This is the more detailed description of this file.
+ *
+ */
+
#ifndef PPRZ_ALGEBRA_DOUBLE_H
#define PPRZ_ALGEBRA_DOUBLE_H
@@ -15,6 +44,9 @@ struct DoubleVect3 {
double z;
};
+/**
+ * @brief Roation quaternion
+ */
struct DoubleQuat {
double qi;
double qx;
@@ -26,20 +58,29 @@ struct DoubleMat33 {
double m[3*3];
};
+/**
+ * @brief rotation matrix
+ */
struct DoubleRMat {
double m[3*3];
};
+/**
+ * @brief euler angles
+ * @details Units: radians */
struct DoubleEulers {
- double phi;
- double theta;
- double psi;
+ double phi; ///< in radians
+ double theta; ///< in radians
+ double psi; ///< in radians
};
+/**
+ * @brief angular rates
+ * @details Units: rad/s^2 */
struct DoubleRates {
- double p;
- double q;
- double r;
+ double p; ///< in rad/s^2
+ double q; ///< in rad/s^2
+ double r; ///< in rad/s^2
};
#define DOUBLE_VECT3_ROUND(_v) DOUBLE_VECT3_RINT(_v, _v)
diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h
index a534bbbea3..5a31345f6c 100644
--- a/sw/airborne/math/pprz_algebra_float.h
+++ b/sw/airborne/math/pprz_algebra_float.h
@@ -1,7 +1,5 @@
/*
- * $Id$
- *
- * Copyright (C) 2008-2010 The Paparazzi Team
+ * Copyright (C) 2008-2011 The Paparazzi Team
*
* This file is part of paparazzi.
*
@@ -16,9 +14,16 @@
* 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.
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ *
+ */
+
+/**
+ * @file pprz_algebra_float.h
+ * @brief Paparazzi floating point algebra.
+ *
+ * This is the more detailed description of this file.
*
*/
@@ -46,6 +51,9 @@ struct FloatVect3 {
float z;
};
+/**
+ * @brief Roation quaternion
+ */
struct FloatQuat {
float qi;
float qx;
@@ -57,20 +65,29 @@ struct FloatMat33 {
float m[3*3];
};
+/**
+ * @brief rotation matrix
+ */
struct FloatRMat {
float m[3*3];
};
+/**
+ * @brief euler angles
+ * @details Units: radians */
struct FloatEulers {
- float phi;
- float theta;
- float psi;
+ float phi; ///< in radians
+ float theta; ///< in radians
+ float psi; ///< in radians
};
+/**
+ * @brief angular rates
+ * @details Units: rad/s^2 */
struct FloatRates {
- float p;
- float q;
- float r;
+ float p; ///< in rad/s^2
+ float q; ///< in rad/s^2
+ float r; ///< in rad/s^2
};
#define FLOAT_ANGLE_NORMALIZE(_a) { \
@@ -136,6 +153,14 @@ struct FloatRates {
(_vo).z = (_v1).x*(_v2).y - (_v1).y*(_v2).x; \
}
+#define FLOAT_VECT3_INTEGRATE_FI(_vo, _dv, _dt) { \
+ (_vo).x += (_dv).x * (_dt); \
+ (_vo).y += (_dv).y * (_dt); \
+ (_vo).z += (_dv).z * (_dt); \
+ }
+
+
+
#define FLOAT_VECT3_NORMALIZE(_v) { \
const float n = FLOAT_VECT3_NORM(_v); \
FLOAT_VECT3_SMUL(_v, _v, 1./n); \
@@ -153,6 +178,34 @@ struct FloatRates {
_ro.r += _v.z * _s; \
}
+
+#define FLOAT_RATES_LIN_CMB(_ro, _r1, _s1, _r2, _s2) { \
+ _ro.p = _s1 * _r1.p + _s2 * _r2.p; \
+ _ro.q = _s1 * _r1.q + _s2 * _r2.q; \
+ _ro.r = _s1 * _r1.r + _s2 * _r2.r; \
+ }
+
+
+#define FLOAT_RATES_SCALE(_ro,_s) { \
+ _ro.p *= _s; \
+ _ro.q *= _s; \
+ _ro.r *= _s; \
+ }
+
+#define FLOAT_RATES_INTEGRATE_FI(_ra, _racc, _dt) { \
+ (_ra).p += (_racc).p * (_dt); \
+ (_ra).q += (_racc).q * (_dt); \
+ (_ra).r += (_racc).r * (_dt); \
+ }
+
+#define FLOAT_RATES_OF_EULER_DOT(_ra, _e, _ed) { \
+ _ra.p = _ed.phi - sinf(_e.theta) *_ed.psi; \
+ _ra.q = cosf(_e.phi)*_ed.theta + sinf(_e.phi)*cosf(_e.theta)*_ed.psi; \
+ _ra.r = -sinf(_e.phi)*_ed.theta + cosf(_e.phi)*cosf(_e.theta)*_ed.psi; \
+ }
+
+
+
/*
* 3x3 matrices
*/
@@ -215,6 +268,7 @@ struct FloatRates {
/* multiply _vin by _rmat, store in _vout */
#define FLOAT_RMAT_VECT3_MUL(_vout, _rmat, _vin) RMAT_VECT3_MUL(_vout, _rmat, _vin)
+#define FLOAT_RMAT_VECT3_TRANSP_MUL(_vout, _rmat, _vin) RMAT_VECT3_TRANSP_MUL(_vout, _rmat, _vin)
#define FLOAT_RMAT_TRANSP_RATEMULT(_vb, _m_b2a, _va) { \
(_vb).p = ( (_m_b2a).m[0]*(_va).p + (_m_b2a).m[3]*(_va).q + (_m_b2a).m[6]*(_va).r); \
@@ -378,6 +432,18 @@ struct FloatRates {
}
#endif
+/* in place first order integration of a rotation matrix */
+#define FLOAT_RMAT_INTEGRATE_FI(_rm, _omega, _dt ) { \
+ struct FloatRMat exp_omega_dt = { \
+ { 1. , dt*omega.r, -dt*omega.q, \
+ -dt*omega.r, 1. , dt*omega.p, \
+ dt*omega.q, -dt*omega.p, 1. }}; \
+ struct FloatRMat R_tdt; \
+ FLOAT_RMAT_COMP(R_tdt, _rm, exp_omega_dt); \
+ memcpy(&(_rm), &R_tdt, sizeof(R_tdt)); \
+ }
+
+
static inline float renorm_factor(float n) {
if (n < 1.5625f && n > 0.64f)
return .5 * (3-n);
@@ -428,7 +494,7 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) {
#define FLOAT_QUAT_NORM(_q) (sqrtf(SQUARE(_q.qi) + SQUARE(_q.qx)+ \
SQUARE(_q.qy) + SQUARE(_q.qz))) \
-#define FLOAT_QUAT_NORMALISE(q) { \
+#define FLOAT_QUAT_NORMALIZE(q) { \
float norm = FLOAT_QUAT_NORM(q); \
if (norm > FLT_MIN) { \
q.qi = q.qi / norm; \
@@ -449,7 +515,7 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) {
#define FLOAT_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c) { \
FLOAT_QUAT_COMP(_a2c, _a2b, _b2c); \
FLOAT_QUAT_WRAP_SHORTEST(_a2c); \
- FLOAT_QUAT_NORMALISE(_a2c); \
+ FLOAT_QUAT_NORMALIZE(_a2c); \
}
/* _a2c = _a2b comp _b2c , aka _a2c = _a2b * _b2c */
@@ -466,7 +532,7 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) {
#define FLOAT_QUAT_COMP_INV_NORM_SHORTEST(_a2b, _a2c, _b2c) { \
FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c); \
FLOAT_QUAT_WRAP_SHORTEST(_a2b); \
- FLOAT_QUAT_NORMALISE(_a2b); \
+ FLOAT_QUAT_NORMALIZE(_a2b); \
}
/* _a2b = _a2c comp_inv _b2c , aka _a2b = _a2c * inv(_b2c) */
@@ -481,7 +547,7 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) {
#define FLOAT_QUAT_INV_COMP_NORM_SHORTEST(_b2c, _a2b, _a2c) { \
FLOAT_QUAT_INV_COMP(_b2c, _a2b, _a2c); \
FLOAT_QUAT_WRAP_SHORTEST(_b2c); \
- FLOAT_QUAT_NORMALISE(_b2c); \
+ FLOAT_QUAT_NORMALIZE(_b2c); \
}
/* _b2c = _a2b inv_comp _a2c , aka _b2c = inv(_a2b) * _a2c */
@@ -492,7 +558,7 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) {
(_b2c).qz = (_a2b).qi*(_a2c).qz - (_a2b).qx*(_a2c).qy + (_a2b).qy*(_a2c).qx - (_a2b).qz*(_a2c).qi; \
}
-#define FLOAT_QUAT_DIFFERENTIAL(q_out, w, dt) { \
+#define FLOAT_QUAT_DIFFERENTIAL(q_out, w, dt) { \
const float v_norm = sqrt((w).p*(w).p + (w).q*(w).q + (w).r*(w).r); \
const float c2 = cos(dt*v_norm/2.0); \
const float s2 = sin(dt*v_norm/2.0); \
@@ -509,6 +575,27 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) {
} \
}
+/* in place quaternion integration with constante rotational velocity */
+#define FLOAT_QUAT_INTEGRATE(_q, _omega, _dt) { \
+ const float no = FLOAT_RATES_NORM(_omega); \
+ if (no > FLT_MIN) { \
+ const float a = 0.5*no*_dt; \
+ const float ca = cosf(a); \
+ const float sa_ov_no = sinf(a)/no; \
+ const float dp = sa_ov_no*_omega.p; \
+ const float dq = sa_ov_no*_omega.q; \
+ const float dr = sa_ov_no*_omega.r; \
+ const float qi = _q.qi; \
+ const float qx = _q.qx; \
+ const float qy = _q.qy; \
+ const float qz = _q.qz; \
+ _q.qi = ca*qi - dp*qx - dq*qy - dr*qz; \
+ _q.qx = dp*qi + ca*qx + dr*qy - dq*qz; \
+ _q.qy = dq*qi - dr*qx + ca*qy + dp*qz; \
+ _q.qz = dr*qi + dq*qx - dp*qy + ca*qz; \
+ } \
+ }
+
#ifdef ALGEBRA_FLOAT_USE_SLOW_FUNCTIONS
#define FLOAT_QUAT_VMULT(v_out, q, v_in) { \
const float qi2 = q.qi*q.qi; \
diff --git a/sw/airborne/math/pprz_algebra_int.h b/sw/airborne/math/pprz_algebra_int.h
index 65c917207d..0a410b2e6a 100644
--- a/sw/airborne/math/pprz_algebra_int.h
+++ b/sw/airborne/math/pprz_algebra_int.h
@@ -1,7 +1,5 @@
/*
- * $Id$
- *
- * Copyright (C) 2008-2010 The Paparazzi Team
+ * Copyright (C) 2008-2011 The Paparazzi Team
*
* This file is part of paparazzi.
*
@@ -16,9 +14,16 @@
* 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.
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ *
+ */
+
+/**
+ * @file pprz_algebra_int.h
+ * @brief Paparazzi fixed point algebra.
+ *
+ * This is the more detailed description of this file.
*
*/
@@ -69,6 +74,9 @@ struct Int32Vect3 {
/* Rotation quaternions */
#define INT32_QUAT_FRAC 15
+/**
+ * @brief Rotation quaternion
+ * @details Units: INT32_QUAT_FRAC */
struct Int32Quat {
int32_t qi;
int32_t qx;
@@ -76,6 +84,15 @@ struct Int32Quat {
int32_t qz;
};
+
+struct Int64Quat {
+ int32_t qi;
+ int32_t qx;
+ int32_t qy;
+ int32_t qz;
+};
+
+
/* Euler angles */
#define INT32_ANGLE_FRAC 12
#define INT32_RATE_FRAC 12
@@ -84,17 +101,17 @@ struct Int32Quat {
#define INT32_ANGLE_PI (int32_t)ANGLE_BFP_OF_REAL( 3.1415926535897932384626433832795029)
#define INT32_ANGLE_2_PI (int32_t)ANGLE_BFP_OF_REAL(2.*3.1415926535897932384626433832795029)
-#define INT32_RAD_OF_DEG(_deg) (int32_t)(((int64_t)_deg * 14964008)/857374503)
-#define INT32_DEG_OF_RAD(_rad) (int32_t)(((int64_t)_rad * 857374503)/14964008)
+#define INT32_RAD_OF_DEG(_deg) (int32_t)(((int64_t)(_deg) * 14964008)/857374503)
+#define INT32_DEG_OF_RAD(_rad) (int32_t)(((int64_t)(_rad) * 857374503)/14964008)
#define INT32_ANGLE_NORMALIZE(_a) { \
- while (_a > INT32_ANGLE_PI) _a -= INT32_ANGLE_2_PI; \
- while (_a < -INT32_ANGLE_PI) _a += INT32_ANGLE_2_PI; \
+ while ((_a) > INT32_ANGLE_PI) (_a) -= INT32_ANGLE_2_PI; \
+ while ((_a) < -INT32_ANGLE_PI) (_a) += INT32_ANGLE_2_PI; \
}
#define INT32_COURSE_NORMALIZE(_a) { \
- while (_a < 0) _a += INT32_ANGLE_2_PI; \
- while (_a >= INT32_ANGLE_2_PI) _a -= INT32_ANGLE_2_PI; \
+ while ((_a) < 0) (_a) += INT32_ANGLE_2_PI; \
+ while ((_a) >= INT32_ANGLE_2_PI) (_a) -= INT32_ANGLE_2_PI; \
}
@@ -104,15 +121,22 @@ struct Int16Eulers {
int16_t psi;
};
+/**
+ * @brief euler angles
+ * @details Units: rad with INT32_ANGLE_FRAC */
struct Int32Eulers {
- int32_t phi;
- int32_t theta;
- int32_t psi;
+ int32_t phi; ///< in rad with INT32_ANGLE_FRAC
+ int32_t theta; ///< in rad with INT32_ANGLE_FRAC
+ int32_t psi; ///< in rad with INT32_ANGLE_FRAC
};
/* Rotation matrix. */
#define INT32_TRIG_FRAC 14
+
+/**
+ * @brief rotation matrix
+ * @details Units: rad with INT32_TRIG_FRAC */
struct Int32RMat {
int32_t m[3*3];
};
@@ -130,12 +154,22 @@ struct Int16Rates {
};
/* Rotational speed */
+/**
+ * @brief angular rates
+ * @details Units: rad/s^2 with INT32_RATE_FRAC */
struct Int32Rates {
- int32_t p;
- int32_t q;
- int32_t r;
+ int32_t p; ///< in rad/s^2 with INT32_RATE_FRAC
+ int32_t q; ///< in rad/s^2 with INT32_RATE_FRAC
+ int32_t r; ///< in rad/s^2 with INT32_RATE_FRAC
};
+struct Int64Rates {
+ int64_t p; ///< in rad/s^2 with INT32_RATE_FRAC
+ int64_t q; ///< in rad/s^2 with INT32_RATE_FRAC
+ int64_t r; ///< in rad/s^2 with INT32_RATE_FRAC
+};
+
+
struct Int64Vect2 {
int64_t x;
int64_t y;
@@ -150,22 +184,22 @@ struct Int64Vect3 {
#define BFP_OF_REAL(_vr, _frac) ((_vr)*(1<<(_frac)))
#define FLOAT_OF_BFP(_vbfp, _frac) ((float)(_vbfp)/(1<<(_frac)))
-#define RATE_BFP_OF_REAL(_af) BFP_OF_REAL(_af, INT32_RATE_FRAC)
+#define RATE_BFP_OF_REAL(_af) BFP_OF_REAL((_af), INT32_RATE_FRAC)
#define RATE_FLOAT_OF_BFP(_ai) FLOAT_OF_BFP((_ai), INT32_RATE_FRAC)
-#define ANGLE_BFP_OF_REAL(_af) BFP_OF_REAL(_af, INT32_ANGLE_FRAC)
+#define ANGLE_BFP_OF_REAL(_af) BFP_OF_REAL((_af), INT32_ANGLE_FRAC)
#define ANGLE_FLOAT_OF_BFP(_ai) FLOAT_OF_BFP((_ai), INT32_ANGLE_FRAC)
-#define QUAT1_BFP_OF_REAL(_qf) BFP_OF_REAL(_qf, INT32_QUAT_FRAC)
-#define QUAT1_FLOAT_OF_BFP(_qi) FLOAT_OF_BFP(_qi, INT32_QUAT_FRAC)
-#define TRIG_BFP_OF_REAL(_tf) BFP_OF_REAL(_tf, INT32_TRIG_FRAC)
-#define TRIG_FLOAT_OF_BFP(_ti) FLOAT_OF_BFP(_ti,INT32_TRIG_FRAC)
-#define POS_BFP_OF_REAL(_af) BFP_OF_REAL(_af, INT32_POS_FRAC)
+#define QUAT1_BFP_OF_REAL(_qf) BFP_OF_REAL((_qf), INT32_QUAT_FRAC)
+#define QUAT1_FLOAT_OF_BFP(_qi) FLOAT_OF_BFP((_qi), INT32_QUAT_FRAC)
+#define TRIG_BFP_OF_REAL(_tf) BFP_OF_REAL((_tf), INT32_TRIG_FRAC)
+#define TRIG_FLOAT_OF_BFP(_ti) FLOAT_OF_BFP((_ti),INT32_TRIG_FRAC)
+#define POS_BFP_OF_REAL(_af) BFP_OF_REAL((_af), INT32_POS_FRAC)
#define POS_FLOAT_OF_BFP(_ai) FLOAT_OF_BFP((_ai), INT32_POS_FRAC)
-#define SPEED_BFP_OF_REAL(_af) BFP_OF_REAL(_af, INT32_SPEED_FRAC)
+#define SPEED_BFP_OF_REAL(_af) BFP_OF_REAL((_af), INT32_SPEED_FRAC)
#define SPEED_FLOAT_OF_BFP(_ai) FLOAT_OF_BFP((_ai), INT32_SPEED_FRAC)
-#define ACCEL_BFP_OF_REAL(_af) BFP_OF_REAL(_af, INT32_ACCEL_FRAC)
+#define ACCEL_BFP_OF_REAL(_af) BFP_OF_REAL((_af), INT32_ACCEL_FRAC)
#define ACCEL_FLOAT_OF_BFP(_ai) FLOAT_OF_BFP((_ai), INT32_ACCEL_FRAC)
-#define MAG_BFP_OF_REAL(_af) BFP_OF_REAL(_af, INT32_MAG_FRAC)
-#define MAG_FLOAT_OF_BFP(_ai) FLOAT_OF_BFP((_ai), INT32_MAG_FRAC)
+#define MAG_BFP_OF_REAL(_af) BFP_OF_REAL((_af), INT32_MAG_FRAC)
+#define MAG_FLOAT_OF_BFP(_ai) FLOAT_OF_BFP((_ai), INT32_MAG_FRAC)
#define INT_MULT_RSHIFT(_a, _b, _r) (((_a)*(_b))>>(_r))
/*
@@ -175,7 +209,7 @@ struct Int64Vect3 {
#define INT_VECT2_ZERO(_v) VECT2_ASSIGN(_v, 0, 0)
#define INT32_VECT2_NORM(n, v) { \
- int32_t n2 = v.x*v.x + v.y*v.y; \
+ int32_t n2 = (v).x*(v).x + (v).y*(v).y; \
INT32_SQRT(n, n2); \
}
@@ -201,29 +235,13 @@ struct Int64Vect3 {
#define INT_VECT3_ZERO(_v) VECT3_ASSIGN(_v, 0, 0, 0)
#define INT32_VECT3_ZERO(_v) VECT3_ASSIGN(_v, 0, 0, 0)
-#define INT32_VECT3_COPY(_o, _i) { \
- _o.x = _i.x; \
- _o.y = _i.y; \
- _o.z = _i.z; \
- }
+#define INT32_VECT3_COPY(_o, _i) VECT3_COPY(_o, _i)
-#define INT32_VECT3_SUM(_c, _a, _b) { \
- _c.x = _a.x + _b.x; \
- _c.y = _a.y + _b.y; \
- _c.z = _a.z + _b.z; \
- }
+#define INT32_VECT3_SUM(_c, _a, _b) VECT3_SUM(_c, _a, _b)
-#define INT32_VECT3_DIFF(_c, _a, _b) { \
- _c.x = _a.x - _b.x; \
- _c.y = _a.y - _b.y; \
- _c.z = _a.z - _b.z; \
- }
+#define INT32_VECT3_DIFF(_c, _a, _b) VECT3_DIFF(_c, _a, _b)
-#define INT32_VECT3_ADD(_a, _b) { \
- _a.x += _b.x; \
- _a.y += _b.y; \
- _a.z += _b.z; \
- }
+#define INT32_VECT3_ADD(_a, _b) VECT3_ADD(_a, _b)
#define INT32_VECT3_SCALE_2(_a, _b, _num, _den) { \
(_a).x = ((_b).x * (_num)) / (_den); \
@@ -231,15 +249,11 @@ struct Int64Vect3 {
(_a).z = ((_b).z * (_num)) / (_den); \
}
-#define INT32_VECT3_SDIV(_a, _b, _s) { \
- (_a).x = (_b).x / (_s); \
- (_a).y = (_b).y / (_s); \
- (_a).z = (_b).z / (_s); \
- }
+#define INT32_VECT3_SDIV(_a, _b, _s) VECT3_SDIV(_a, _b, _s)
#define INT32_VECT3_NORM(n, v) { \
- int32_t n2 = v.x*v.x + v.y*v.y + v.z*v.z; \
+ int32_t n2 = (v).x*(v).x + (v).y*(v).y + (v).z*(v).z; \
INT32_SQRT(n, n2); \
}
@@ -255,6 +269,11 @@ struct Int64Vect3 {
(_o).z = ((_i).z << (_l)); \
}
+#define INT32_VECT3_CROSS_PRODUCT(_vo, _v1, _v2) { \
+ (_vo).x = (_v1).y*(_v2).z - (_v1).z*(_v2).y; \
+ (_vo).y = (_v1).z*(_v2).x - (_v1).x*(_v2).z; \
+ (_vo).z = (_v1).x*(_v2).y - (_v1).y*(_v2).x; \
+ }
@@ -262,27 +281,27 @@ struct Int64Vect3 {
* 3x3 Matrices
*/
#define INT32_MAT33_ZERO(_m) { \
- MAT33_ELMT(_m, 0, 0) = 0; \
- MAT33_ELMT(_m, 0, 1) = 0; \
- MAT33_ELMT(_m, 0, 2) = 0; \
- MAT33_ELMT(_m, 1, 0) = 0; \
- MAT33_ELMT(_m, 1, 1) = 0; \
- MAT33_ELMT(_m, 1, 2) = 0; \
- MAT33_ELMT(_m, 2, 0) = 0; \
- MAT33_ELMT(_m, 2, 1) = 0; \
- MAT33_ELMT(_m, 2, 2) = 0; \
+ MAT33_ELMT((_m), 0, 0) = 0; \
+ MAT33_ELMT((_m), 0, 1) = 0; \
+ MAT33_ELMT((_m), 0, 2) = 0; \
+ MAT33_ELMT((_m), 1, 0) = 0; \
+ MAT33_ELMT((_m), 1, 1) = 0; \
+ MAT33_ELMT((_m), 1, 2) = 0; \
+ MAT33_ELMT((_m), 2, 0) = 0; \
+ MAT33_ELMT((_m), 2, 1) = 0; \
+ MAT33_ELMT((_m), 2, 2) = 0; \
}
#define INT32_MAT33_DIAG(_m, _d00, _d11, _d22) { \
- MAT33_ELMT(_m, 0, 0) = _d00; \
- MAT33_ELMT(_m, 0, 1) = 0; \
- MAT33_ELMT(_m, 0, 2) = 0; \
- MAT33_ELMT(_m, 1, 0) = 0; \
- MAT33_ELMT(_m, 1, 1) = _d11; \
- MAT33_ELMT(_m, 1, 2) = 0; \
- MAT33_ELMT(_m, 2, 0) = 0; \
- MAT33_ELMT(_m, 2, 1) = 0; \
- MAT33_ELMT(_m, 2, 2) = _d22; \
+ MAT33_ELMT((_m), 0, 0) = (_d00); \
+ MAT33_ELMT((_m), 0, 1) = 0; \
+ MAT33_ELMT((_m), 0, 2) = 0; \
+ MAT33_ELMT((_m), 1, 0) = 0; \
+ MAT33_ELMT((_m), 1, 1) = (_d11); \
+ MAT33_ELMT((_m), 1, 2) = 0; \
+ MAT33_ELMT((_m), 2, 0) = 0; \
+ MAT33_ELMT((_m), 2, 1) = 0; \
+ MAT33_ELMT((_m), 2, 2) = (_d22); \
}
@@ -301,28 +320,28 @@ struct Int64Vect3 {
/* _m_a2c = _m_a2b comp _m_b2c , aka _m_a2c = _m_b2c * _m_a2b */
#define INT32_RMAT_COMP(_m_a2c, _m_a2b, _m_b2c) { \
- _m_a2c.m[0] = (_m_b2c.m[0]*_m_a2b.m[0] + _m_b2c.m[1]*_m_a2b.m[3] + _m_b2c.m[2]*_m_a2b.m[6])>>INT32_TRIG_FRAC; \
- _m_a2c.m[1] = (_m_b2c.m[0]*_m_a2b.m[1] + _m_b2c.m[1]*_m_a2b.m[4] + _m_b2c.m[2]*_m_a2b.m[7])>>INT32_TRIG_FRAC; \
- _m_a2c.m[2] = (_m_b2c.m[0]*_m_a2b.m[2] + _m_b2c.m[1]*_m_a2b.m[5] + _m_b2c.m[2]*_m_a2b.m[8])>>INT32_TRIG_FRAC; \
- _m_a2c.m[3] = (_m_b2c.m[3]*_m_a2b.m[0] + _m_b2c.m[4]*_m_a2b.m[3] + _m_b2c.m[5]*_m_a2b.m[6])>>INT32_TRIG_FRAC; \
- _m_a2c.m[4] = (_m_b2c.m[3]*_m_a2b.m[1] + _m_b2c.m[4]*_m_a2b.m[4] + _m_b2c.m[5]*_m_a2b.m[7])>>INT32_TRIG_FRAC; \
- _m_a2c.m[5] = (_m_b2c.m[3]*_m_a2b.m[2] + _m_b2c.m[4]*_m_a2b.m[5] + _m_b2c.m[5]*_m_a2b.m[8])>>INT32_TRIG_FRAC; \
- _m_a2c.m[6] = (_m_b2c.m[6]*_m_a2b.m[0] + _m_b2c.m[7]*_m_a2b.m[3] + _m_b2c.m[8]*_m_a2b.m[6])>>INT32_TRIG_FRAC; \
- _m_a2c.m[7] = (_m_b2c.m[6]*_m_a2b.m[1] + _m_b2c.m[7]*_m_a2b.m[4] + _m_b2c.m[8]*_m_a2b.m[7])>>INT32_TRIG_FRAC; \
- _m_a2c.m[8] = (_m_b2c.m[6]*_m_a2b.m[2] + _m_b2c.m[7]*_m_a2b.m[5] + _m_b2c.m[8]*_m_a2b.m[8])>>INT32_TRIG_FRAC; \
+ (_m_a2c).m[0] = ((_m_b2c).m[0]*(_m_a2b).m[0] + (_m_b2c).m[1]*(_m_a2b).m[3] + (_m_b2c).m[2]*(_m_a2b).m[6])>>INT32_TRIG_FRAC; \
+ (_m_a2c).m[1] = ((_m_b2c).m[0]*(_m_a2b).m[1] + (_m_b2c).m[1]*(_m_a2b).m[4] + (_m_b2c).m[2]*(_m_a2b).m[7])>>INT32_TRIG_FRAC; \
+ (_m_a2c).m[2] = ((_m_b2c).m[0]*(_m_a2b).m[2] + (_m_b2c).m[1]*(_m_a2b).m[5] + (_m_b2c).m[2]*(_m_a2b).m[8])>>INT32_TRIG_FRAC; \
+ (_m_a2c).m[3] = ((_m_b2c).m[3]*(_m_a2b).m[0] + (_m_b2c).m[4]*(_m_a2b).m[3] + (_m_b2c).m[5]*(_m_a2b).m[6])>>INT32_TRIG_FRAC; \
+ (_m_a2c).m[4] = ((_m_b2c).m[3]*(_m_a2b).m[1] + (_m_b2c).m[4]*(_m_a2b).m[4] + (_m_b2c).m[5]*(_m_a2b).m[7])>>INT32_TRIG_FRAC; \
+ (_m_a2c).m[5] = ((_m_b2c).m[3]*(_m_a2b).m[2] + (_m_b2c).m[4]*(_m_a2b).m[5] + (_m_b2c).m[5]*(_m_a2b).m[8])>>INT32_TRIG_FRAC; \
+ (_m_a2c).m[6] = ((_m_b2c).m[6]*(_m_a2b).m[0] + (_m_b2c).m[7]*(_m_a2b).m[3] + (_m_b2c).m[8]*(_m_a2b).m[6])>>INT32_TRIG_FRAC; \
+ (_m_a2c).m[7] = ((_m_b2c).m[6]*(_m_a2b).m[1] + (_m_b2c).m[7]*(_m_a2b).m[4] + (_m_b2c).m[8]*(_m_a2b).m[7])>>INT32_TRIG_FRAC; \
+ (_m_a2c).m[8] = ((_m_b2c).m[6]*(_m_a2b).m[2] + (_m_b2c).m[7]*(_m_a2b).m[5] + (_m_b2c).m[8]*(_m_a2b).m[8])>>INT32_TRIG_FRAC; \
}
/* _m_a2b = _m_a2c comp_inv _m_b2c , aka _m_a2b = inv(_m_b2c) * _m_a2c */
#define INT32_RMAT_COMP_INV(_m_a2b, _m_a2c, _m_b2c) { \
- _m_a2b.m[0] = (_m_b2c.m[0]*_m_a2c.m[0] + _m_b2c.m[3]*_m_a2c.m[3] + _m_b2c.m[6]*_m_a2c.m[6])>>INT32_TRIG_FRAC; \
- _m_a2b.m[1] = (_m_b2c.m[0]*_m_a2c.m[1] + _m_b2c.m[3]*_m_a2c.m[4] + _m_b2c.m[6]*_m_a2c.m[7])>>INT32_TRIG_FRAC; \
- _m_a2b.m[2] = (_m_b2c.m[0]*_m_a2c.m[2] + _m_b2c.m[3]*_m_a2c.m[5] + _m_b2c.m[6]*_m_a2c.m[8])>>INT32_TRIG_FRAC; \
- _m_a2b.m[3] = (_m_b2c.m[1]*_m_a2c.m[0] + _m_b2c.m[4]*_m_a2c.m[3] + _m_b2c.m[7]*_m_a2c.m[6])>>INT32_TRIG_FRAC; \
- _m_a2b.m[4] = (_m_b2c.m[1]*_m_a2c.m[1] + _m_b2c.m[4]*_m_a2c.m[4] + _m_b2c.m[7]*_m_a2c.m[7])>>INT32_TRIG_FRAC; \
- _m_a2b.m[5] = (_m_b2c.m[1]*_m_a2c.m[2] + _m_b2c.m[4]*_m_a2c.m[5] + _m_b2c.m[7]*_m_a2c.m[8])>>INT32_TRIG_FRAC; \
- _m_a2b.m[6] = (_m_b2c.m[2]*_m_a2c.m[0] + _m_b2c.m[5]*_m_a2c.m[3] + _m_b2c.m[8]*_m_a2c.m[6])>>INT32_TRIG_FRAC; \
- _m_a2b.m[7] = (_m_b2c.m[2]*_m_a2c.m[1] + _m_b2c.m[5]*_m_a2c.m[4] + _m_b2c.m[8]*_m_a2c.m[7])>>INT32_TRIG_FRAC; \
- _m_a2b.m[8] = (_m_b2c.m[2]*_m_a2c.m[2] + _m_b2c.m[5]*_m_a2c.m[5] + _m_b2c.m[8]*_m_a2c.m[8])>>INT32_TRIG_FRAC; \
+ (_m_a2b).m[0] = ((_m_b2c).m[0]*(_m_a2c).m[0] + (_m_b2c).m[3]*(_m_a2c).m[3] + (_m_b2c).m[6]*(_m_a2c).m[6])>>INT32_TRIG_FRAC; \
+ (_m_a2b).m[1] = ((_m_b2c).m[0]*(_m_a2c).m[1] + (_m_b2c).m[3]*(_m_a2c).m[4] + (_m_b2c).m[6]*(_m_a2c).m[7])>>INT32_TRIG_FRAC; \
+ (_m_a2b).m[2] = ((_m_b2c).m[0]*(_m_a2c).m[2] + (_m_b2c).m[3]*(_m_a2c).m[5] + (_m_b2c).m[6]*(_m_a2c).m[8])>>INT32_TRIG_FRAC; \
+ (_m_a2b).m[3] = ((_m_b2c).m[1]*(_m_a2c).m[0] + (_m_b2c).m[4]*(_m_a2c).m[3] + (_m_b2c).m[7]*(_m_a2c).m[6])>>INT32_TRIG_FRAC; \
+ (_m_a2b).m[4] = ((_m_b2c).m[1]*(_m_a2c).m[1] + (_m_b2c).m[4]*(_m_a2c).m[4] + (_m_b2c).m[7]*(_m_a2c).m[7])>>INT32_TRIG_FRAC; \
+ (_m_a2b).m[5] = ((_m_b2c).m[1]*(_m_a2c).m[2] + (_m_b2c).m[4]*(_m_a2c).m[5] + (_m_b2c).m[7]*(_m_a2c).m[8])>>INT32_TRIG_FRAC; \
+ (_m_a2b).m[6] = ((_m_b2c).m[2]*(_m_a2c).m[0] + (_m_b2c).m[5]*(_m_a2c).m[3] + (_m_b2c).m[8]*(_m_a2c).m[6])>>INT32_TRIG_FRAC; \
+ (_m_a2b).m[7] = ((_m_b2c).m[2]*(_m_a2c).m[1] + (_m_b2c).m[5]*(_m_a2c).m[4] + (_m_b2c).m[8]*(_m_a2c).m[7])>>INT32_TRIG_FRAC; \
+ (_m_a2b).m[8] = ((_m_b2c).m[2]*(_m_a2c).m[2] + (_m_b2c).m[5]*(_m_a2c).m[5] + (_m_b2c).m[8]*(_m_a2c).m[8])>>INT32_TRIG_FRAC; \
}
/* _vb = _m_a2b * _va */
@@ -363,47 +382,47 @@ struct Int64Vect3 {
const int32_t one = TRIG_BFP_OF_REAL( 1); \
const int32_t two = TRIG_BFP_OF_REAL( 2); \
/* dcm00 = 1.0 - 2.*( qy2 + qz2 ); */ \
- _rm.m[0] = one - INT_MULT_RSHIFT( two, (qy2+qz2), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \
+ (_rm).m[0] = one - INT_MULT_RSHIFT( two, (qy2+qz2), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \
/* dcm01 = 2.*( qxqy + qiqz ); */ \
- _rm.m[1] = INT_MULT_RSHIFT( two, (qxqy+qiqz), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \
+ (_rm).m[1] = INT_MULT_RSHIFT( two, (qxqy+qiqz), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \
/* dcm02 = 2.*( qxqz - qiqy ); */ \
- _rm.m[2] = INT_MULT_RSHIFT( two, (qxqz-qiqy), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \
+ (_rm).m[2] = INT_MULT_RSHIFT( two, (qxqz-qiqy), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \
/* dcm10 = 2.*( qxqy - qiqz ); */ \
- _rm.m[3] = INT_MULT_RSHIFT( two, (qxqy-qiqz), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \
+ (_rm).m[3] = INT_MULT_RSHIFT( two, (qxqy-qiqz), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \
/* dcm11 = 1.0 - 2.*(qx2+qz2); */ \
- _rm.m[4] = one - INT_MULT_RSHIFT( two, (qx2+qz2), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \
+ (_rm).m[4] = one - INT_MULT_RSHIFT( two, (qx2+qz2), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \
/* dcm12 = 2.*( qyqz + qiqx ); */ \
- _rm.m[5] = INT_MULT_RSHIFT( two, (qyqz+qiqx), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \
+ (_rm).m[5] = INT_MULT_RSHIFT( two, (qyqz+qiqx), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \
/* dcm20 = 2.*( qxqz + qiqy ); */ \
- _rm.m[6] = INT_MULT_RSHIFT( two, (qxqz+qiqy), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \
+ (_rm).m[6] = INT_MULT_RSHIFT( two, (qxqz+qiqy), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \
/* dcm21 = 2.*( qyqz - qiqx ); */ \
- _rm.m[7] = INT_MULT_RSHIFT( two, (qyqz-qiqx), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \
+ (_rm).m[7] = INT_MULT_RSHIFT( two, (qyqz-qiqx), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \
/* dcm22 = 1.0 - 2.*( qx2 + qy2 ); */ \
- _rm.m[8] = one - INT_MULT_RSHIFT( two, (qx2+qy2), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \
+ (_rm).m[8] = one - INT_MULT_RSHIFT( two, (qx2+qy2), INT32_TRIG_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC); \
}
#else
#define INT32_RMAT_OF_QUAT(_rm, _q) { \
const int32_t _2qi2_m1 = INT_MULT_RSHIFT((_q).qi,(_q).qi, INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1)-TRIG_BFP_OF_REAL( 1); \
- _rm.m[0] = INT_MULT_RSHIFT((_q).qx,(_q).qx, INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
- _rm.m[4] = INT_MULT_RSHIFT((_q).qy,(_q).qy, INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
- _rm.m[8] = INT_MULT_RSHIFT((_q).qz,(_q).qz, INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
+ (_rm).m[0] = INT_MULT_RSHIFT((_q).qx,(_q).qx, INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
+ (_rm).m[4] = INT_MULT_RSHIFT((_q).qy,(_q).qy, INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
+ (_rm).m[8] = INT_MULT_RSHIFT((_q).qz,(_q).qz, INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
\
const int32_t _2qiqx = INT_MULT_RSHIFT((_q).qi,(_q).qx, INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
const int32_t _2qiqy = INT_MULT_RSHIFT((_q).qi,(_q).qy, INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
const int32_t _2qiqz = INT_MULT_RSHIFT((_q).qi,(_q).qz, INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
- _rm.m[1] = INT_MULT_RSHIFT((_q).qx,(_q).qy, INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
- _rm.m[2] = INT_MULT_RSHIFT((_q).qx,(_q).qz, INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
- _rm.m[5] = INT_MULT_RSHIFT((_q).qy,(_q).qz, INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
+ (_rm).m[1] = INT_MULT_RSHIFT((_q).qx,(_q).qy, INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
+ (_rm).m[2] = INT_MULT_RSHIFT((_q).qx,(_q).qz, INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
+ (_rm).m[5] = INT_MULT_RSHIFT((_q).qy,(_q).qz, INT32_QUAT_FRAC+INT32_QUAT_FRAC-INT32_TRIG_FRAC-1); \
\
- _rm.m[0] += _2qi2_m1; \
- _rm.m[3] = _rm.m[1]-_2qiqz; \
- _rm.m[6] = _rm.m[2]+_2qiqy; \
- _rm.m[7] = _rm.m[5]-_2qiqx; \
- _rm.m[4] += _2qi2_m1; \
- _rm.m[1] += _2qiqz; \
- _rm.m[2] -= _2qiqy; \
- _rm.m[5] += _2qiqx; \
- _rm.m[8] += _2qi2_m1; \
+ (_rm).m[0] += _2qi2_m1; \
+ (_rm).m[3] = (_rm).m[1]-_2qiqz; \
+ (_rm).m[6] = (_rm).m[2]+_2qiqy; \
+ (_rm).m[7] = (_rm).m[5]-_2qiqx; \
+ (_rm).m[4] += _2qi2_m1; \
+ (_rm).m[1] += _2qiqz; \
+ (_rm).m[2] -= _2qiqy; \
+ (_rm).m[5] += _2qiqx; \
+ (_rm).m[8] += _2qi2_m1; \
}
#endif
@@ -508,36 +527,31 @@ struct Int64Vect3 {
*/
#define INT32_QUAT_ZERO(_q) { \
- _q.qi = QUAT1_BFP_OF_REAL(1); \
- _q.qx = 0; \
- _q.qy = 0; \
- _q.qz = 0; \
+ (_q).qi = QUAT1_BFP_OF_REAL(1); \
+ (_q).qx = 0; \
+ (_q).qy = 0; \
+ (_q).qz = 0; \
}
-#define INT32_QUAT_INVERT(_qo, _qi) { \
- (_qo).qi = (_qi).qi; \
- (_qo).qx = -(_qi).qx; \
- (_qo).qy = -(_qi).qy; \
- (_qo).qz = -(_qi).qz; \
- }
+#define INT32_QUAT_INVERT(_qo, _qi) QUAT_INVERT(_qo, _qi)
#define INT32_QUAT_NORM(n, q) { \
- int32_t n2 = q.qi*q.qi + q.qx*q.qx + q.qy*q.qy + q.qz*q.qz; \
+ int32_t n2 = (q).qi*(q).qi + (q).qx*(q).qx + (q).qy*(q).qy + (q).qz*(q).qz; \
INT32_SQRT(n, n2); \
}
#define INT32_QUAT_WRAP_SHORTEST(q) { \
- if (q.qi < 0) \
+ if ((q).qi < 0) \
QUAT_EXPLEMENTARY(q,q); \
}
-#define INT32_QUAT_NORMALISE(q) { \
+#define INT32_QUAT_NORMALIZE(q) { \
int32_t n; \
INT32_QUAT_NORM(n, q); \
- q.qi = q.qi * QUAT1_BFP_OF_REAL(1) / n; \
- q.qx = q.qx * QUAT1_BFP_OF_REAL(1) / n; \
- q.qy = q.qy * QUAT1_BFP_OF_REAL(1) / n; \
- q.qz = q.qz * QUAT1_BFP_OF_REAL(1) / n; \
+ (q).qi = (q).qi * QUAT1_BFP_OF_REAL(1) / n; \
+ (q).qx = (q).qx * QUAT1_BFP_OF_REAL(1) / n; \
+ (q).qy = (q).qy * QUAT1_BFP_OF_REAL(1) / n; \
+ (q).qz = (q).qz * QUAT1_BFP_OF_REAL(1) / n; \
}
/* _a2c = _a2b comp _b2c , aka _a2c = _b2c * _a2b */
@@ -564,18 +578,20 @@ struct Int64Vect3 {
(_b2c).qz = ((_a2b).qi*(_a2c).qz - (_a2b).qx*(_a2c).qy + (_a2b).qy*(_a2c).qx - (_a2b).qz*(_a2c).qi)>>INT32_QUAT_FRAC; \
}
+
+
#ifdef ALGEBRA_INT_USE_SLOW_FUNCTIONS
#define INT32_QUAT_VMULT(v_out, q, v_in) { \
- const int32_t qi2 = (q.qi*q.qi)>>INT32_QUAT_FRAC; \
- const int32_t qx2 = (q.qx*q.qx)>>INT32_QUAT_FRAC; \
- const int32_t qy2 = (q.qy*q.qy)>>INT32_QUAT_FRAC; \
- const int32_t qz2 = (q.qz*q.qz)>>INT32_QUAT_FRAC; \
- const int32_t qiqx = (q.qi*q.qx)>>INT32_QUAT_FRAC; \
- const int32_t qiqy = (q.qi*q.qy)>>INT32_QUAT_FRAC; \
- const int32_t qiqz = (q.qi*q.qz)>>INT32_QUAT_FRAC; \
- const int32_t qxqy = (q.qx*q.qy)>>INT32_QUAT_FRAC; \
- const int32_t qxqz = (q.qx*q.qz)>>INT32_QUAT_FRAC; \
- const int32_t qyqz = (q.qy*q.qz)>>INT32_QUAT_FRAC; \
+ const int32_t qi2 = ((q).qi*(q).qi)>>INT32_QUAT_FRAC; \
+ const int32_t qx2 = ((q).qx*(q).qx)>>INT32_QUAT_FRAC; \
+ const int32_t qy2 = ((q).qy*(q).qy)>>INT32_QUAT_FRAC; \
+ const int32_t qz2 = ((q).qz*(q).qz)>>INT32_QUAT_FRAC; \
+ const int32_t qiqx = ((q).qi*(q).qx)>>INT32_QUAT_FRAC; \
+ const int32_t qiqy = ((q).qi*(q).qy)>>INT32_QUAT_FRAC; \
+ const int32_t qiqz = ((q).qi*(q).qz)>>INT32_QUAT_FRAC; \
+ const int32_t qxqy = ((q).qx*(q).qy)>>INT32_QUAT_FRAC; \
+ const int32_t qxqz = ((q).qx*(q).qz)>>INT32_QUAT_FRAC; \
+ const int32_t qyqz = ((q).qy*(q).qz)>>INT32_QUAT_FRAC; \
const int32_t m00 = qi2 + qx2 - qy2 - qz2; \
const int32_t m01 = 2 * (qxqy + qiqz ); \
const int32_t m02 = 2 * (qxqz - qiqy ); \
@@ -585,25 +601,25 @@ struct Int64Vect3 {
const int32_t m20 = 2 * (qxqz + qiqy ); \
const int32_t m21 = 2 * (qyqz - qiqx ); \
const int32_t m22 = qi2 - qx2 - qy2 + qz2; \
- v_out.x = (m00 * v_in.x + m01 * v_in.y + m02 * v_in.z)>>INT32_QUAT_FRAC; \
- v_out.y = (m10 * v_in.x + m11 * v_in.y + m12 * v_in.z)>>INT32_QUAT_FRAC; \
- v_out.z = (m20 * v_in.x + m21 * v_in.y + m22 * v_in.z)>>INT32_QUAT_FRAC; \
+ (v_out).x = (m00 * (v_in).x + m01 * (v_in).y + m02 * (v_in).z)>>INT32_QUAT_FRAC; \
+ (v_out).y = (m10 * (v_in).x + m11 * (v_in).y + m12 * (v_in).z)>>INT32_QUAT_FRAC; \
+ (v_out).z = (m20 * (v_in).x + m21 * (v_in).y + m22 * (v_in).z)>>INT32_QUAT_FRAC; \
}
#else
#define INT32_QUAT_VMULT(v_out, q, v_in) { \
- const int32_t _2qi2_m1 = ((q.qi*q.qi)>>(INT32_QUAT_FRAC-1)) - QUAT1_BFP_OF_REAL( 1); \
- const int32_t _2qx2 = (q.qx*q.qx)>>(INT32_QUAT_FRAC-1); \
- const int32_t _2qy2 = (q.qy*q.qy)>>(INT32_QUAT_FRAC-1); \
- const int32_t _2qz2 = (q.qz*q.qz)>>(INT32_QUAT_FRAC-1); \
- const int32_t _2qiqx = (q.qi*q.qx)>>(INT32_QUAT_FRAC-1); \
- const int32_t _2qiqy = (q.qi*q.qy)>>(INT32_QUAT_FRAC-1); \
- const int32_t _2qiqz = (q.qi*q.qz)>>(INT32_QUAT_FRAC-1); \
- const int32_t m01 = ((q.qx*q.qy)>>(INT32_QUAT_FRAC-1)) + _2qiqz; \
- const int32_t m02 = ((q.qx*q.qz)>>(INT32_QUAT_FRAC-1)) - _2qiqy; \
- const int32_t m12 = ((q.qy*q.qz)>>(INT32_QUAT_FRAC-1)) + _2qiqx; \
- v_out.x = (_2qi2_m1*v_in.x + _2qx2 * v_in.x + m01 * v_in.y + m02 * v_in.z)>>INT32_QUAT_FRAC; \
- v_out.y = (_2qi2_m1*v_in.y + m01 * v_in.x -2*_2qiqz*v_in.x+ _2qy2 * v_in.y + m12 * v_in.z)>>INT32_QUAT_FRAC; \
- v_out.z = (_2qi2_m1*v_in.z + m02 * v_in.x +2*_2qiqy*v_in.x+ m12 * v_in.y -2*_2qiqx*v_in.y+ _2qz2 * v_in.z)>>INT32_QUAT_FRAC; \
+ const int32_t _2qi2_m1 = (((q).qi*(q).qi)>>(INT32_QUAT_FRAC-1)) - QUAT1_BFP_OF_REAL( 1); \
+ const int32_t _2qx2 = ((q).qx*(q).qx)>>(INT32_QUAT_FRAC-1); \
+ const int32_t _2qy2 = ((q).qy*(q).qy)>>(INT32_QUAT_FRAC-1); \
+ const int32_t _2qz2 = ((q).qz*(q).qz)>>(INT32_QUAT_FRAC-1); \
+ const int32_t _2qiqx = ((q).qi*(q).qx)>>(INT32_QUAT_FRAC-1); \
+ const int32_t _2qiqy = ((q).qi*(q).qy)>>(INT32_QUAT_FRAC-1); \
+ const int32_t _2qiqz = ((q).qi*(q).qz)>>(INT32_QUAT_FRAC-1); \
+ const int32_t m01 = (((q).qx*(q).qy)>>(INT32_QUAT_FRAC-1)) + _2qiqz; \
+ const int32_t m02 = (((q).qx*(q).qz)>>(INT32_QUAT_FRAC-1)) - _2qiqy; \
+ const int32_t m12 = (((q).qy*(q).qz)>>(INT32_QUAT_FRAC-1)) + _2qiqx; \
+ (v_out).x = (_2qi2_m1*(v_in).x + _2qx2 * (v_in).x + m01 * (v_in).y + m02 * (v_in).z)>>INT32_QUAT_FRAC; \
+ (v_out).y = (_2qi2_m1*(v_in).y + m01 * (v_in).x -2*_2qiqz*(v_in).x + _2qy2 * (v_in).y + m12 * (v_in).z)>>INT32_QUAT_FRAC; \
+ (v_out).z = (_2qi2_m1*(v_in).z + m02 * (v_in).x +2*_2qiqy*(v_in).x+ m12 * (v_in).y -2*_2qiqx*(v_in).y+ _2qz2 * (v_in).z)>>INT32_QUAT_FRAC; \
}
#endif
@@ -653,14 +669,14 @@ struct Int64Vect3 {
int32_t two_qi; \
INT32_SQRT(two_qi, (two_qi_two<> (_r)); \
+ (_o).q = ((_i).q >> (_r)); \
+ (_o).r = ((_i).r >> (_r)); \
+ }
+
+#define INT_RATES_LSHIFT(_o, _i, _r) { \
+ (_o).p = ((_i).p << (_r)); \
+ (_o).q = ((_i).q << (_r)); \
+ (_o).r = ((_i).r << (_r)); \
+ }
+
+
+
+#define INT32_RATES_OF_EULERS_DOT_321(_r, _e, _ed) { \
\
int32_t sphi; \
PPRZ_ITRIG_SIN(sphi, (_e).phi); \
@@ -855,21 +897,21 @@ struct Int64Vect3 {
*/
#define INT32_SQRT_MAX_ITER 40
#define INT32_SQRT(_out,_in) { \
- if (_in == 0) \
- _out = 0; \
+ if ((_in) == 0) \
+ (_out) = 0; \
else { \
uint32_t s1, s2; \
uint8_t iter = 0; \
s2 = _in; \
do { \
s1 = s2; \
- s2 = _in / s1; \
+ s2 = (_in) / s1; \
s2 += s1; \
s2 /= 2; \
iter++; \
} \
while( ( (s1-s2) > 1) && (iter < INT32_SQRT_MAX_ITER)); \
- _out = s2; \
+ (_out) = s2; \
} \
}
@@ -884,16 +926,16 @@ struct Int64Vect3 {
const int32_t c2 = 3 * INT32_ANGLE_PI_4; \
const int32_t abs_y = abs(_y) + 1; \
int32_t r; \
- if ( _x >= 0) { \
- r = ((_x-abs_y)<>R_FRAC); \
+ if ( (_x) >= 0) { \
+ r = (((_x)-abs_y)<>R_FRAC); \
} \
else { \
- r = ((_x+abs_y)<>R_FRAC); \
+ r = (((_x)+abs_y)<>R_FRAC); \
} \
- if (_y<0) \
- _a = -_a; \
+ if ((_y)<0) \
+ (_a) = -(_a); \
}
@@ -902,18 +944,18 @@ struct Int64Vect3 {
const int32_t c2 = 3 * INT32_ANGLE_PI_4; \
const int32_t abs_y = abs(_y) + 1; \
int32_t r; \
- if ( _x >= 0) { \
- r = ((_x-abs_y)<= 0) { \
+ r = (((_x)-abs_y)<>R_FRAC; \
int32_t tmp1 = ((r2 * (int32_t)ANGLE_BFP_OF_REAL(0.1963))>>INT32_ANGLE_FRAC) - ANGLE_BFP_OF_REAL(0.9817); \
- _a = ((tmp1 * r)>>R_FRAC) + c1; \
+ (_a) = ((tmp1 * r)>>R_FRAC) + c1; \
} \
else { \
- r = ((_x+abs_y)<>R_FRAC); \
+ r = (((_x)+abs_y)<>R_FRAC); \
} \
- if (_y<0) \
- _a = -_a; \
+ if ((_y)<0) \
+ (_a) = -(_a); \
}
diff --git a/sw/airborne/math/pprz_geodetic.h b/sw/airborne/math/pprz_geodetic.h
index 218352752e..08f0a84ed7 100644
--- a/sw/airborne/math/pprz_geodetic.h
+++ b/sw/airborne/math/pprz_geodetic.h
@@ -8,10 +8,10 @@
(_po).z = -(_pi).z; \
}
-#define LLA_ASSIGN(_pos,_lat,_lon,_alt){ \
- (_pos).lat = (_lat); \
- (_pos).lon = (_lon); \
- (_pos).alt = (_alt); \
+#define LLA_ASSIGN(_pos,_lat,_lon,_alt){ \
+ (_pos).lat = (_lat); \
+ (_pos).lon = (_lon); \
+ (_pos).alt = (_alt); \
}
#define LLA_COPY(_pos1,_pos2){ \
diff --git a/sw/airborne/math/pprz_geodetic_double.c b/sw/airborne/math/pprz_geodetic_double.c
index d8570396b0..5a411f62ff 100644
--- a/sw/airborne/math/pprz_geodetic_double.c
+++ b/sw/airborne/math/pprz_geodetic_double.c
@@ -1,6 +1,8 @@
#include "pprz_geodetic_double.h"
#include
+#include "std.h" /* for RadOfDeg */
+
void ltp_def_from_ecef_d(struct LtpDef_d* def, struct EcefCoor_d* ecef) {
@@ -68,10 +70,10 @@ void ecef_of_lla_d(struct EcefCoor_d* ecef, struct LlaCoor_d* lla) {
static const double f = 1./298.257223563; /* reciprocal flattening */
const double e2 = 2.*f-(f*f); /* first eccentricity squared */
- const double sin_lat = sinf(lla->lat);
- const double cos_lat = cosf(lla->lat);
- const double sin_lon = sinf(lla->lon);
- const double cos_lon = cosf(lla->lon);
+ const double sin_lat = sin(lla->lat);
+ const double cos_lat = cos(lla->lat);
+ const double sin_lon = sin(lla->lon);
+ const double cos_lon = cos(lla->lon);
const double chi = sqrtf(1. - e2*sin_lat*sin_lat);
const double a_chi = a / chi;
@@ -125,7 +127,7 @@ void ecef_of_ned_vect_d(struct EcefCoor_d* ecef, struct LtpDef_d* def, struct Ne
ecef_of_enu_vect_d(ecef, def, &enu);
}
-
+/* geocentric latitude of geodetic latitude */
double gc_of_gd_lat_d(double gd_lat, double hmsl) {
const double a = 6378137.0; /* earth semimajor axis in meters */
const double f = 1./298.257223563; /* reciprocal flattening */
@@ -137,3 +139,92 @@ double gc_of_gd_lat_d(double gd_lat, double hmsl) {
+/* Computation for the WGS84 geoid only */
+#define E 0.08181919106
+#define K0 0.9996
+#define DELTA_EAST 500000.
+#define DELTA_NORTH 0.
+#define A 6378137.0
+#define N (K0*A)
+
+#define LambdaOfUtmZone(utm_zone) RadOfDeg((utm_zone-1)*6-180+3)
+
+static const float serie_coeff_proj_mercator[5] = {
+ 0.99832429842242842444,
+ 0.00083632803657738403,
+ 0.00000075957783563707,
+ 0.00000000119563131778,
+ 0.00000000000241079916
+};
+
+static inline double isometric_latitude(double phi, double e) {
+ return log (tan (M_PI_4 + phi / 2.0)) - e / 2.0 * log((1.0 + e * sin(phi)) / (1.0 - e * sin(phi)));
+}
+
+static inline double isometric_latitude_fast(double phi) {
+ return log (tan (M_PI_4 + phi / 2.0));
+}
+
+static inline double inverse_isometric_latitude(double lat, double e, double epsilon) {
+ double exp_l = exp(lat);
+ double phi0 = 2 * atan(exp_l) - M_PI_2;
+ double phi_;
+ uint8_t max_iter = 3; /* To be sure to return */
+
+ do {
+ phi_ = phi0;
+ double sin_phi = e * sin(phi_);
+ phi0 = 2 * atan (pow((1 + sin_phi) / (1. - sin_phi), e/2.) * exp_l) - M_PI_2;
+ max_iter--;
+ }
+ while (max_iter && fabs(phi_ - phi0) > epsilon);
+
+ return phi0;
+}
+
+#define CI(v) { \
+ double tmp = v.x; \
+ v.x = -v.y; \
+ v.y = tmp; \
+ }
+
+#define CExp(v) { \
+ double e = exp(v.x); \
+ v.x = e*cosf(v.y); \
+ v.y = e*sinf(v.y); \
+ }
+
+#define CSin(v) { \
+ CI(v); \
+ struct DoubleVect2 vstar = {-v.x, -v.y}; \
+ CExp(v); \
+ CExp(vstar); \
+ VECT2_SUB(v, vstar); \
+ VECT2_SMUL(v, v, -0.5); \
+ CI(v); \
+ }
+
+void lla_of_utm(struct LlaCoor_d* out, struct UTMCoor_d* in) {
+
+ // struct DoubleVect2 v = {in->east - YS, in->north - XS};
+ struct DoubleVect2 v = {in->north - DELTA_NORTH, in->east - DELTA_EAST};
+ double scale = 1 / N / serie_coeff_proj_mercator[0];
+ VECT2_SMUL(v, v, scale);
+
+ // first order taylor serie of something ?
+ struct DoubleVect2 v1;
+ VECT2_SMUL(v1, v, 2.);
+ CSin(v1)
+ VECT2_SMUL(v1, v1, serie_coeff_proj_mercator[1]);
+ VECT2_SUB(v, v1);
+
+ double lambda_c = LambdaOfUtmZone(in->zone);
+ out->lon = lambda_c + atan(sinh(v.y) / cos(v.x));
+ double phi = asin (sin(v.x) / cosh(v.y));
+ double il = isometric_latitude_fast(phi);
+ out->lat = inverse_isometric_latitude(il, E, 1e-8);
+
+ // copy alt above reference ellipsoid
+ out->alt = in->alt;
+
+}
diff --git a/sw/airborne/math/pprz_geodetic_double.h b/sw/airborne/math/pprz_geodetic_double.h
index 5fc9389216..a0c7128fee 100644
--- a/sw/airborne/math/pprz_geodetic_double.h
+++ b/sw/airborne/math/pprz_geodetic_double.h
@@ -1,45 +1,103 @@
+/*
+ * Copyright (C) 2008-2011 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, see
+ * .
+ *
+ */
+
+/**
+ * @file pprz_geodetic_double.h
+ * @brief Paparazzi double-precision floating point math for geodetic calculations.
+ *
+ * This is the more detailed description of this file.
+ *
+ */
+
#ifndef PPRZ_GEODETIC_DOUBLE_H
#define PPRZ_GEODETIC_DOUBLE_H
#include "pprz_geodetic.h"
#include "pprz_algebra_double.h"
+#include "std.h"
-/* Earth Centered Earth Fixed in meters */
+/**
+ * @brief vector in EarthCenteredEarthFixed coordinates
+ * @details Origin at center of mass of the Earth. Z-axis is pointing north,
+ * the x-axis intersects the sphere of the earth at 0° latitude (Equator)
+ * and 0° longitude (Greenwich). Y-axis completes it to right-hand system.
+ * Units: meters */
struct EcefCoor_d {
- double x;
- double y;
- double z;
+ double x; ///< in meters
+ double y; ///< in meters
+ double z; ///< in meters
};
-/* lon, lat in radians */
-/* alt in meters */
+/**
+ * @brief vector in Latitude, Longitude and Altitude
+ * @details Units lat,lon: radians
+ * Unit alt: meters above MSL
+ */
struct LlaCoor_d {
- double lon;
- double lat;
- double alt;
+ double lon; ///< in radians
+ double lat; ///< in radians
+ double alt; ///< in meters above WGS84 reference ellipsoid
};
-/* North East Down local tangeant plane */
+/**
+ * @brief vector in North East Down coordinates
+ * Units: meters */
struct NedCoor_d {
- double x;
- double y;
- double z;
+ double x; ///< in meters
+ double y; ///< in meters
+ double z; ///< in meters
};
-/* East North Up local tangeant plane */
+/**
+ * @brief vector in East North Up coordinates
+ * Units: meters */
struct EnuCoor_d {
- double x;
- double y;
- double z;
+ double x; ///< in meters
+ double y; ///< in meters
+ double z; ///< in meters
};
-/* Local tangeant plane reference */
+/**
+ * @brief position in UTM coordinates
+ * Units: meters */
+struct UTMCoor_d {
+ double north; ///< in meters
+ double east; ///< in meters
+ double alt; ///< in meters above WGS84 reference ellipsoid
+ uint8_t zone; ///< UTM zone number
+};
+
+/**
+ * @brief definition of the local (flat earth) coordinate system
+ * @details Defines the origin of the local coordinate system
+ * in ECEF and LLA coordinates and the roation matrix from
+ * ECEF to local frame */
struct LtpDef_d {
- struct EcefCoor_d ecef;
- struct LlaCoor_d lla;
- struct DoubleMat33 ltp_of_ecef;
+ struct EcefCoor_d ecef; ///< origin of local frame in ECEF
+ struct LlaCoor_d lla; ///< origin of local frame in LLA
+ struct DoubleMat33 ltp_of_ecef; ///< rotation from ECEF to local frame
+ double hmsl; ///< height in meters above mean sea level
};
+extern void lla_of_utm(struct LlaCoor_d* out, struct UTMCoor_d* in);
extern void ltp_def_from_ecef_d(struct LtpDef_d* def, struct EcefCoor_d* ecef);
extern void lla_of_ecef_d(struct LlaCoor_d* out, struct EcefCoor_d* in);
extern void ecef_of_lla_d(struct EcefCoor_d* out, struct LlaCoor_d* in);
diff --git a/sw/airborne/math/pprz_geodetic_float.c b/sw/airborne/math/pprz_geodetic_float.c
index 6fef1d0411..9d5f4fdb5e 100644
--- a/sw/airborne/math/pprz_geodetic_float.c
+++ b/sw/airborne/math/pprz_geodetic_float.c
@@ -3,6 +3,9 @@
#include "pprz_algebra_float.h"
#include
+/* for ecef_of_XX functions the double versions are needed */
+#include "pprz_geodetic_double.h"
+
void ltp_def_from_ecef_f(struct LtpDef_f* def, struct EcefCoor_f* ecef) {
/* store the origin of the tangeant plane */
@@ -16,7 +19,7 @@ void ltp_def_from_ecef_f(struct LtpDef_f* def, struct EcefCoor_f* ecef) {
const float cos_lon = cosf(def->lla.lon);
def->ltp_of_ecef.m[0] = -sin_lon;
def->ltp_of_ecef.m[1] = cos_lon;
- def->ltp_of_ecef.m[2] = 0.;
+ def->ltp_of_ecef.m[2] = 0.; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */
def->ltp_of_ecef.m[3] = -sin_lat*cos_lon;
def->ltp_of_ecef.m[4] = -sin_lat*sin_lon;
def->ltp_of_ecef.m[5] = cos_lat;
@@ -26,14 +29,28 @@ void ltp_def_from_ecef_f(struct LtpDef_f* def, struct EcefCoor_f* ecef) {
}
-#if 0
-void init_ltp_ref_from_lla_f(struct LtpRef_f* def, struct LlaCoor_f* ref_pos) {
- def->lla.lon = ref_pos->lon;
- def->lla.lat = ref_pos->lat;
- /* compute ecef */
+void ltp_def_from_lla_f(struct LtpDef_f* def, struct LlaCoor_f* lla) {
+ /* store the origin of the tangeant plane */
+ LLA_COPY(def->lla, *lla);
+ /* compute the ecef representation of the origin */
+ ecef_of_lla_f(&def->ecef, &def->lla);
+ /* store the rotation matrix */
+ const float sin_lat = sinf(def->lla.lat);
+ const float cos_lat = cosf(def->lla.lat);
+ const float sin_lon = sinf(def->lla.lon);
+ const float cos_lon = cosf(def->lla.lon);
+
+ def->ltp_of_ecef.m[0] = -sin_lon;
+ def->ltp_of_ecef.m[1] = cos_lon;
+ def->ltp_of_ecef.m[2] = 0.; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */
+ def->ltp_of_ecef.m[3] = -sin_lat*cos_lon;
+ def->ltp_of_ecef.m[4] = -sin_lat*sin_lon;
+ def->ltp_of_ecef.m[5] = cos_lat;
+ def->ltp_of_ecef.m[6] = cos_lat*cos_lon;
+ def->ltp_of_ecef.m[7] = cos_lat*sin_lon;
+ def->ltp_of_ecef.m[8] = sin_lat;
}
-#endif
void enu_of_ecef_point_f(struct EnuCoor_f* enu, struct LtpDef_f* def, struct EcefCoor_f* ecef) {
struct EcefCoor_f delta;
@@ -58,21 +75,79 @@ void ned_of_ecef_vect_f(struct NedCoor_f* ned, struct LtpDef_f* def, struct Ecef
ENU_OF_TO_NED(*ned, enu);
}
-/* not enought precision with float - use double */
-# if 0
+void enu_of_lla_point_f(struct EnuCoor_f* enu, struct LtpDef_f* def, struct LlaCoor_f* lla) {
+ struct EcefCoor_f ecef;
+ ecef_of_lla_f(&ecef,lla);
+ enu_of_ecef_point_f(enu,def,&ecef);
+}
+
+void ned_of_lla_point_f(struct NedCoor_f* ned, struct LtpDef_f* def, struct LlaCoor_f* lla) {
+ struct EcefCoor_f ecef;
+ ecef_of_lla_f(&ecef,lla);
+ ned_of_ecef_point_f(ned,def,&ecef);
+}
+
+/*
+ * not enought precision with float - use double
+ */
void ecef_of_enu_point_f(struct EcefCoor_f* ecef, struct LtpDef_f* def, struct EnuCoor_f* enu) {
- MAT33_VECT3_TRANSP_MUL(*ecef, def->ltp_of_ecef.m, *enu);
- VECT3_ADD(*ecef, def->ecef);
+ /* convert used floats to double */
+ struct DoubleMat33 ltp_of_ecef_d;
+ ltp_of_ecef_d.m[0] = (double) def->ltp_of_ecef.m[0];
+ ltp_of_ecef_d.m[1] = (double) def->ltp_of_ecef.m[1];
+ ltp_of_ecef_d.m[2] = (double) def->ltp_of_ecef.m[2];
+ ltp_of_ecef_d.m[3] = (double) def->ltp_of_ecef.m[3];
+ ltp_of_ecef_d.m[4] = (double) def->ltp_of_ecef.m[4];
+ ltp_of_ecef_d.m[5] = (double) def->ltp_of_ecef.m[5];
+ ltp_of_ecef_d.m[6] = (double) def->ltp_of_ecef.m[6];
+ ltp_of_ecef_d.m[7] = (double) def->ltp_of_ecef.m[7];
+ ltp_of_ecef_d.m[8] = (double) def->ltp_of_ecef.m[8];
+ struct EnuCoor_f enu_d;
+ enu_d.x = (double) enu->x;
+ enu_d.y = (double) enu->y;
+ enu_d.z = (double) enu->z;
+
+ /* compute in double */
+ struct EcefCoor_d ecef_d;
+ MAT33_VECT3_TRANSP_MUL(ecef_d, ltp_of_ecef_d, enu_d);
+
+ /* convert result back to float and add it*/
+ ecef->x = (float) ecef_d.x + def->ecef.x;
+ ecef->y = (float) ecef_d.y + def->ecef.y;
+ ecef->z = (float) ecef_d.z + def->ecef.z;
}
void ecef_of_ned_point_f(struct EcefCoor_f* ecef, struct LtpDef_f* def, struct NedCoor_f* ned) {
struct EnuCoor_f enu;
ENU_OF_TO_NED(enu, *ned);
- ecef_of_enu_pos_f(ecef, def, &enu);
+ ecef_of_enu_point_f(ecef, def, &enu);
}
void ecef_of_enu_vect_f(struct EcefCoor_f* ecef, struct LtpDef_f* def, struct EnuCoor_f* enu) {
- MAT33_VECT3_TRANSP_MUL(*ecef, def->ltp_of_ecef.m, *enu);
+ /* convert used floats to double */
+ struct DoubleMat33 ltp_of_ecef_d;
+ ltp_of_ecef_d.m[0] = (double) def->ltp_of_ecef.m[0];
+ ltp_of_ecef_d.m[1] = (double) def->ltp_of_ecef.m[1];
+ ltp_of_ecef_d.m[2] = (double) def->ltp_of_ecef.m[2];
+ ltp_of_ecef_d.m[3] = (double) def->ltp_of_ecef.m[3];
+ ltp_of_ecef_d.m[4] = (double) def->ltp_of_ecef.m[4];
+ ltp_of_ecef_d.m[5] = (double) def->ltp_of_ecef.m[5];
+ ltp_of_ecef_d.m[6] = (double) def->ltp_of_ecef.m[6];
+ ltp_of_ecef_d.m[7] = (double) def->ltp_of_ecef.m[7];
+ ltp_of_ecef_d.m[8] = (double) def->ltp_of_ecef.m[8];
+ struct EnuCoor_f enu_d;
+ enu_d.x = (double) enu->x;
+ enu_d.y = (double) enu->y;
+ enu_d.z = (double) enu->z;
+
+ /* compute in double */
+ struct EcefCoor_d ecef_d;
+ MAT33_VECT3_TRANSP_MUL(ecef_d, ltp_of_ecef_d, enu_d);
+
+ /* convert result back to float*/
+ ecef->x = (float) ecef_d.x;
+ ecef->y = (float) ecef_d.y;
+ ecef->z = (float) ecef_d.z;
}
void ecef_of_ned_vect_f(struct EcefCoor_f* ecef, struct LtpDef_f* def, struct NedCoor_f* ned) {
@@ -80,7 +155,7 @@ void ecef_of_ned_vect_f(struct EcefCoor_f* ecef, struct LtpDef_f* def, struct Ne
ENU_OF_TO_NED(enu, *ned);
ecef_of_enu_vect_f(ecef, def, &enu);
}
-#endif
+/* end use double versions */
diff --git a/sw/airborne/math/pprz_geodetic_float.h b/sw/airborne/math/pprz_geodetic_float.h
index 4eef0066ee..e40e502fb5 100644
--- a/sw/airborne/math/pprz_geodetic_float.h
+++ b/sw/airborne/math/pprz_geodetic_float.h
@@ -1,61 +1,108 @@
+/*
+ * Copyright (C) 2008-2011 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, see
+ * .
+ *
+ */
+
+/**
+ * @file pprz_geodetic_float.h
+ * @brief Paparazzi floating point math for geodetic calculations.
+ *
+ * This is the more detailed description of this file.
+ *
+ */
+
#ifndef PPRZ_GEODETIC_FLOAT_H
#define PPRZ_GEODETIC_FLOAT_H
#include "pprz_geodetic.h"
#include "pprz_algebra_float.h"
-/* Earth Centered Earth Fixed in meters */
+/**
+ * @brief vector in EarthCenteredEarthFixed coordinates
+ * @details Origin at center of mass of the Earth. Z-axis is pointing north,
+ * the x-axis intersects the sphere of the earth at 0° latitude (Equator)
+ * and 0° longitude (Greenwich). Y-axis completes it to right-hand system.
+ * Units: meters */
struct EcefCoor_f {
- float x;
- float y;
- float z;
+ float x; ///< in meters
+ float y; ///< in meters
+ float z; ///< in meters
};
-/* lon, lat in radians */
-/* alt in meters */
+/**
+ * @brief vector in Latitude, Longitude and Altitude
+ * @details Units lat,lon: radians
+ * Unit alt: meters above MSL
+ */
struct LlaCoor_f {
- float lon;
- float lat;
- float alt;
+ float lon; ///< in radians
+ float lat; ///< in radians
+ float alt; ///< in meters above WGS84 reference ellipsoid
};
-/* North East Down local tangeant plane */
+/**
+ * @brief vector in North East Down coordinates
+ * Units: meters */
struct NedCoor_f {
- float x;
- float y;
- float z;
+ float x; ///< in meters
+ float y; ///< in meters
+ float z; ///< in meters
};
-/* East North Up local tangeant plane */
+/**
+ * @brief vector in East North Up coordinates
+ * Units: meters */
struct EnuCoor_f {
- float x;
- float y;
- float z;
+ float x; ///< in meters
+ float y; ///< in meters
+ float z; ///< in meters
};
-/* Local tangeant plane reference */
+/**
+ * @brief definition of the local (flat earth) coordinate system
+ * @details Defines the origin of the local coordinate system
+ * in ECEF and LLA coordinates and the roation matrix from
+ * ECEF to local frame */
struct LtpDef_f {
- struct EcefCoor_f ecef;
- struct LlaCoor_f lla;
- struct FloatMat33 ltp_of_ecef;
+ struct EcefCoor_f ecef; ///< origin of local frame in ECEF
+ struct LlaCoor_f lla; ///< origin of local frame in LLA
+ struct FloatMat33 ltp_of_ecef; ///< rotation from ECEF to local frame
+ float hmsl; ///< Height above mean sea level in meters
};
extern void ltp_def_from_ecef_f(struct LtpDef_f* def, struct EcefCoor_f* ecef);
-//extern void ltp_def_from_lla_f(struct LtpDef_f* def, struct LlaCoor_f* lla);
+extern void ltp_def_from_lla_f(struct LtpDef_f* def, struct LlaCoor_f* lla);
extern void lla_of_ecef_f(struct LlaCoor_f* out, struct EcefCoor_f* in);
extern void ecef_of_lla_f(struct EcefCoor_f* out, struct LlaCoor_f* in);
extern void enu_of_ecef_point_f(struct EnuCoor_f* enu, struct LtpDef_f* def, struct EcefCoor_f* ecef);
extern void ned_of_ecef_point_f(struct NedCoor_f* ned, struct LtpDef_f* def, struct EcefCoor_f* ecef);
extern void enu_of_ecef_vect_f(struct EnuCoor_f* enu, struct LtpDef_f* def, struct EcefCoor_f* ecef);
extern void ned_of_ecef_vect_f(struct NedCoor_f* ned, struct LtpDef_f* def, struct EcefCoor_f* ecef);
+extern void enu_of_lla_point_f(struct EnuCoor_f* enu, struct LtpDef_f* def, struct LlaCoor_f* lla);
+extern void ned_of_lla_point_f(struct NedCoor_f* ned, struct LtpDef_f* def, struct LlaCoor_f* lla);
/* not enought precision with floats - used the double version */
-#if 0
extern void ecef_of_enu_point_f(struct EcefCoor_f* ecef, struct LtpDef_f* def, struct EnuCoor_f* enu);
extern void ecef_of_ned_point_f(struct EcefCoor_f* ecef, struct LtpDef_f* def, struct NedCoor_f* ned);
extern void ecef_of_enu_vect_f(struct EcefCoor_f* ecef, struct LtpDef_f* def, struct EnuCoor_f* enu);
extern void ecef_of_ned_vect_f(struct EcefCoor_f* ecef, struct LtpDef_f* def, struct NedCoor_f* ned);
-#endif
+/* end use double versions */
#endif /* PPRZ_GEODETIC_FLOAT_H */
diff --git a/sw/airborne/math/pprz_geodetic_int.c b/sw/airborne/math/pprz_geodetic_int.c
index e5675607c5..99153cc38c 100644
--- a/sw/airborne/math/pprz_geodetic_int.c
+++ b/sw/airborne/math/pprz_geodetic_int.c
@@ -24,10 +24,6 @@
#include "pprz_geodetic_int.h"
#include "pprz_algebra_int.h"
-#define CM_OF_M(_m) ((_m)*1e2)
-#define M_OF_CM(_cm) ((_cm)/1e2)
-#define EM7RAD_OF_RAD(_r) (_r*1e7)
-#define RAD_OF_EM7RAD(_r) (_r/1e7)
#define HIGH_RES_TRIG_FRAC 20
void ltp_def_from_ecef_i(struct LtpDef_i* def, struct EcefCoor_i* ecef) {
@@ -53,7 +49,7 @@ void ltp_def_from_ecef_i(struct LtpDef_i* def, struct EcefCoor_i* ecef) {
def->ltp_of_ecef.m[0] = -sin_lon;
def->ltp_of_ecef.m[1] = cos_lon;
- def->ltp_of_ecef.m[2] = 0;
+ def->ltp_of_ecef.m[2] = 0; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */
def->ltp_of_ecef.m[3] = (int32_t)((-(int64_t)sin_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC);
def->ltp_of_ecef.m[4] = (int32_t)((-(int64_t)sin_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC);
def->ltp_of_ecef.m[5] = cos_lat;
@@ -63,9 +59,38 @@ void ltp_def_from_ecef_i(struct LtpDef_i* def, struct EcefCoor_i* ecef) {
}
+void ltp_def_from_lla_i(struct LtpDef_i* def, struct LlaCoor_i* lla) {
-//void init_ltp_from_lla_i(struct LtpRef_i* ref_param, struct LlaCoor_i* ref_pos) {
-//}
+ /* store the origin of the tangeant plane */
+ LLA_COPY(def->lla, *lla);
+ /* compute the ecef representation of the origin */
+ ecef_of_lla_i(&def->ecef, &def->lla);
+ /* store the rotation matrix */
+
+#if 1
+ int32_t sin_lat = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)def->lla.lat)), HIGH_RES_TRIG_FRAC));
+ int32_t cos_lat = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)def->lla.lat)), HIGH_RES_TRIG_FRAC));
+ int32_t sin_lon = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)def->lla.lon)), HIGH_RES_TRIG_FRAC));
+ int32_t cos_lon = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)def->lla.lon)), HIGH_RES_TRIG_FRAC));
+#else
+ int32_t sin_lat = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)def->lla.lat)), HIGH_RES_TRIG_FRAC));
+ int32_t cos_lat = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)def->lla.lat)), HIGH_RES_TRIG_FRAC));
+ int32_t sin_lon = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)def->lla.lon)), HIGH_RES_TRIG_FRAC));
+ int32_t cos_lon = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)def->lla.lon)), HIGH_RES_TRIG_FRAC));
+#endif
+
+
+ def->ltp_of_ecef.m[0] = -sin_lon;
+ def->ltp_of_ecef.m[1] = cos_lon;
+ def->ltp_of_ecef.m[2] = 0; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */
+ def->ltp_of_ecef.m[3] = (int32_t)((-(int64_t)sin_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC);
+ def->ltp_of_ecef.m[4] = (int32_t)((-(int64_t)sin_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC);
+ def->ltp_of_ecef.m[5] = cos_lat;
+ def->ltp_of_ecef.m[6] = (int32_t)(( (int64_t)cos_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC);
+ def->ltp_of_ecef.m[7] = (int32_t)(( (int64_t)cos_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC);
+ def->ltp_of_ecef.m[8] = sin_lat;
+
+}
void enu_of_ecef_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef) {
@@ -73,14 +98,14 @@ void enu_of_ecef_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct Ece
VECT3_DIFF(delta, *ecef, def->ecef);
const int64_t tmpx = (int64_t)def->ltp_of_ecef.m[0]*delta.x +
(int64_t)def->ltp_of_ecef.m[1]*delta.y +
- 0;
+ 0; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */
enu->x = (int32_t)(tmpx>>HIGH_RES_TRIG_FRAC);
const int64_t tmpy = (int64_t)def->ltp_of_ecef.m[3]*delta.x +
(int64_t)def->ltp_of_ecef.m[4]*delta.y +
(int64_t)def->ltp_of_ecef.m[5]*delta.z;
enu->y = (int32_t)(tmpy>>HIGH_RES_TRIG_FRAC);
const int64_t tmpz = (int64_t)def->ltp_of_ecef.m[6]*delta.x +
- (int64_t)def->ltp_of_ecef.m[7]*delta.y +
+ (int64_t)def->ltp_of_ecef.m[7]*delta.y +
(int64_t)def->ltp_of_ecef.m[8]*delta.z;
enu->z = (int32_t)(tmpz>>HIGH_RES_TRIG_FRAC);
@@ -99,14 +124,14 @@ void enu_of_ecef_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct Ecef
const int64_t tmpx = (int64_t)def->ltp_of_ecef.m[0]*ecef->x +
(int64_t)def->ltp_of_ecef.m[1]*ecef->y +
- 0;
+ 0; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */
enu->x = (int32_t)(tmpx>>HIGH_RES_TRIG_FRAC);
const int64_t tmpy = (int64_t)def->ltp_of_ecef.m[3]*ecef->x +
(int64_t)def->ltp_of_ecef.m[4]*ecef->y +
(int64_t)def->ltp_of_ecef.m[5]*ecef->z;
enu->y = (int32_t)(tmpy>>HIGH_RES_TRIG_FRAC);
const int64_t tmpz = (int64_t)def->ltp_of_ecef.m[6]*ecef->x +
- (int64_t)def->ltp_of_ecef.m[7]*ecef->y +
+ (int64_t)def->ltp_of_ecef.m[7]*ecef->y +
(int64_t)def->ltp_of_ecef.m[8]*ecef->z;
enu->z = (int32_t)(tmpz>>HIGH_RES_TRIG_FRAC);
@@ -114,15 +139,57 @@ void enu_of_ecef_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct Ecef
void ned_of_ecef_vect_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef) {
-
struct EnuCoor_i enu;
enu_of_ecef_vect_i(&enu, def, ecef);
ENU_OF_TO_NED(*ned, enu);
+}
+/* check if resolution of INT32_TRIG_FRAC (14) is enough here */
+void ecef_of_enu_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu) {
+ INT32_RMAT_TRANSP_VMULT(*ecef, def->ltp_of_ecef, *enu);
+ INT32_VECT3_ADD(*ecef, def->ecef);
+}
+
+void ecef_of_ned_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned) {
+ struct EnuCoor_i enu;
+ ENU_OF_TO_NED(enu, *ned);
+ ecef_of_enu_point_i(ecef, def, &enu);
+}
+
+void ecef_of_enu_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu) {
+ INT32_RMAT_TRANSP_VMULT(*ecef, def->ltp_of_ecef, *enu);
+}
+
+void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned) {
+ struct EnuCoor_i enu;
+ ENU_OF_TO_NED(enu, *ned);
+ ecef_of_enu_vect_i(ecef, def, &enu);
}
+void enu_of_lla_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct LlaCoor_i* lla) {
+ struct EcefCoor_i ecef;
+ ecef_of_lla_i(&ecef,lla);
+ enu_of_ecef_point_i(enu,def,&ecef);
+}
+void ned_of_lla_point_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct LlaCoor_i* lla) {
+ struct EcefCoor_i ecef;
+ ecef_of_lla_i(&ecef,lla);
+ ned_of_ecef_point_i(ned,def,&ecef);
+}
+
+void enu_of_lla_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct LlaCoor_i* lla) {
+ struct EcefCoor_i ecef;
+ ecef_of_lla_i(&ecef,lla);
+ enu_of_ecef_vect_i(enu,def,&ecef);
+}
+
+void ned_of_lla_vect_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct LlaCoor_i* lla) {
+ struct EcefCoor_i ecef;
+ ecef_of_lla_i(&ecef,lla);
+ ned_of_ecef_vect_i(ned,def,&ecef);
+}
/*
For now we cheat and call the floating point version
@@ -164,19 +231,3 @@ void ecef_of_lla_i(struct EcefCoor_i* out, struct LlaCoor_i* in) {
out->z = (int32_t)CM_OF_M(out_d.z);
}
-
-//#include "stdio.h"
-void enu_of_lla_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct LlaCoor_i* lla) {
- struct EcefCoor_i ecef;
- ecef_of_lla_i(&ecef,lla);
- //printf("sim %d %d %d, def %d %d %d\n",ecef.x,ecef.y,ecef.z,def->ecef.x,def->ecef.y,def->ecef.z);
- //printf("sim lla def %d %d %d\n",def->lla.lat,def->lla.lon,def->lla.alt);
- enu_of_ecef_point_i(enu,def,&ecef);
-}
-
-void ned_of_lla_point_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct LlaCoor_i* lla) {
- struct EcefCoor_i ecef;
- ecef_of_lla_i(&ecef,lla);
- ned_of_ecef_point_i(ned,def,&ecef);
-}
-
diff --git a/sw/airborne/math/pprz_geodetic_int.h b/sw/airborne/math/pprz_geodetic_int.h
index 93a640387d..91a5770fd6 100644
--- a/sw/airborne/math/pprz_geodetic_int.h
+++ b/sw/airborne/math/pprz_geodetic_int.h
@@ -1,3 +1,32 @@
+/*
+ * Copyright (C) 2008-2011 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, see
+ * .
+ *
+ */
+
+/**
+ * @file pprz_geodetic_int.h
+ * @brief Paparazzi fixed point math for geodetic calculations.
+ *
+ * This is the more detailed description of this file.
+ *
+ */
+
#ifndef PPRZ_GEODETIC_INT_H
#define PPRZ_GEODETIC_INT_H
@@ -6,47 +35,72 @@
#include "std.h"
#include "pprz_algebra_int.h"
-/*
- Earth Centered Earth Fixed in centimeters
-*/
+
+/**
+ * @brief vector in EarthCenteredEarthFixed coordinates
+ * @details Origin at center of mass of the Earth. Z-axis is pointing north,
+ * the x-axis intersects the sphere of the earth at 0° latitude (Equator)
+ * and 0° longitude (Greenwich). Y-axis completes it to right-hand system.
+ * Units: centimeters */
struct EcefCoor_i {
- int32_t x;
- int32_t y;
- int32_t z;
+ int32_t x; ///< in centimeters
+ int32_t y; ///< in centimeters
+ int32_t z; ///< in centimeters
};
-/* lon, lat in radians*1e7 */
-/* alt in centimeters */
+/**
+ * @brief vector in Latitude, Longitude and Altitude
+ * @details Units lat,lon: radians*1e7
+ * Unit alt: centimeters above MSL
+ */
struct LlaCoor_i {
- int32_t lon;
- int32_t lat;
- int32_t alt;
+ int32_t lon; ///< in radians*1e7
+ int32_t lat; ///< in radians*1e7
+ int32_t alt; ///< in centimeters above WGS84 reference ellipsoid
};
-/* North East Down local tangeant plane */
+/**
+ * @brief vector in North East Down coordinates
+ */
struct NedCoor_i {
int32_t x;
int32_t y;
int32_t z;
};
-/* East North Up local tangeant plane */
+/**
+ * @brief vector in East North Up coordinates
+ */
struct EnuCoor_i {
int32_t x;
int32_t y;
int32_t z;
};
-/* Local tangeant plane definition */
+/**
+ * @brief position in UTM coordinates
+ */
+struct UTMCoor_i {
+ int32_t north; ///< in centimeters
+ int32_t east; ///< in centimeters
+ int32_t alt; ///< in centimeters above WGS84 reference ellipsoid
+ uint8_t zone; ///< UTM zone number
+};
+
+/**
+ * @brief definition of the local (flat earth) coordinate system
+ * @details Defines the origin of the local coordinate system
+ * in ECEF and LLA coordinates and the roation matrix from
+ * ECEF to local frame */
struct LtpDef_i {
- struct EcefCoor_i ecef; /* Reference point in ecef */
- struct LlaCoor_i lla; /* Reference point in lla */
- struct Int32Mat33 ltp_of_ecef; /* Rotation matrix */
- int32_t hmsl; /* Height above mean sea level */
+ struct EcefCoor_i ecef; ///< Reference point in ecef
+ struct LlaCoor_i lla; ///< Reference point in lla
+ struct Int32Mat33 ltp_of_ecef; ///< Rotation matrix
+ int32_t hmsl; ///< Height above mean sea level
};
extern void ltp_def_from_ecef_i(struct LtpDef_i* def, struct EcefCoor_i* ecef);
-//extern void ltp_def_from_lla_i(struct LtpRef_i* def, struct LlaCoor_i* lla);
+extern void ltp_def_from_lla_i(struct LtpDef_i* def, struct LlaCoor_i* lla);
extern void lla_of_ecef_i(struct LlaCoor_i* out, struct EcefCoor_i* in);
extern void ecef_of_lla_i(struct EcefCoor_i* out, struct LlaCoor_i* in);
extern void enu_of_ecef_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef);
@@ -55,18 +109,69 @@ extern void enu_of_ecef_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, stru
extern void ned_of_ecef_vect_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef);
extern void enu_of_lla_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct LlaCoor_i* lla);
extern void ned_of_lla_point_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct LlaCoor_i* lla);
+extern void enu_of_lla_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct LlaCoor_i* lla);
+extern void ned_of_lla_vect_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct LlaCoor_i* lla);
+extern void ecef_of_enu_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu);
+extern void ecef_of_ned_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned);
+extern void ecef_of_enu_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu);
+extern void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned);
+
+#define CM_OF_M(_m) ((_m)*1e2)
+#define M_OF_CM(_cm) ((_cm)/1e2)
+#define EM7RAD_OF_RAD(_r) (_r*1e7)
+#define RAD_OF_EM7RAD(_r) (_r/1e7)
#define INT32_VECT3_ENU_OF_NED(_o, _i) { \
- _o.x = _i.y; \
- _o.y = _i.x; \
- _o.z = -_i.z; \
+ (_o).x = (_i).y; \
+ (_o).y = (_i).x; \
+ (_o).z = -(_i).z; \
}
#define INT32_VECT3_NED_OF_ENU(_o, _i) INT32_VECT3_ENU_OF_NED(_o,_i)
+#define ECEF_BFP_OF_REAL(_o, _i) { \
+ (_o).x = (int32_t)CM_OF_M((_i).x); \
+ (_o).y = (int32_t)CM_OF_M((_i).y); \
+ (_o).z = (int32_t)CM_OF_M((_i).z); \
+ }
+
+#define ECEF_FLOAT_OF_BFP(_o, _i) { \
+ (_o).x = (float)M_OF_CM((_i).x); \
+ (_o).y = (float)M_OF_CM((_i).y); \
+ (_o).z = (float)M_OF_CM((_i).z); \
+ }
+
+#define LLA_BFP_OF_REAL(_o, _i) { \
+ (_o).lat = (int32_t)EM7RAD_OF_RAD((_i).lat); \
+ (_o).lon = (int32_t)EM7RAD_OF_RAD((_i).lon); \
+ (_o).alt = (int32_t)CM_OF_M((_i).alt); \
+ }
+
+#define LLA_FLOAT_OF_BFP(_o, _i) { \
+ (_o).lat = (float)RAD_OF_EM7RAD((_i).lat); \
+ (_o).lon = (float)RAD_OF_EM7RAD((_i).lon); \
+ (_o).alt = (float)M_OF_CM((_i).alt); \
+ }
+
+#define NED_BFP_OF_REAL(_o, _i) { \
+ (_o).x = POS_BFP_OF_REAL((_i).x); \
+ (_o).y = POS_BFP_OF_REAL((_i).y); \
+ (_o).z = POS_BFP_OF_REAL((_i).z); \
+ }
+
+#define ENU_BFP_OF_REAL(_o, _i) NED_BFP_OF_REAL(_o, _i)
+
+#define NED_FLOAT_OF_BFP(_o, _i) { \
+ (_o).x = POS_FLOAT_OF_BFP((_i).x); \
+ (_o).y = POS_FLOAT_OF_BFP((_i).y); \
+ (_o).z = POS_FLOAT_OF_BFP((_i).z); \
+ }
+
+#define ENU_FLOAT_OF_BFP(_o, _i) NED_FLOAT_OF_BFP(_o, _i)
+
#define INT32_VECT2_ENU_OF_NED(_o, _i) { \
- _o.x = _i.y; \
- _o.y = _i.x; \
+ (_o).x = (_i).y; \
+ (_o).y = (_i).x; \
}
#define INT32_VECT2_NED_OF_ENU(_o, _i) INT32_VECT2_ENU_OF_NED(_o,_i)
diff --git a/sw/airborne/mcu.c b/sw/airborne/mcu.c
index e0853e4bab..1e8fee07d7 100644
--- a/sw/airborne/mcu.c
+++ b/sw/airborne/mcu.c
@@ -48,6 +48,9 @@
#ifdef USE_SPI
#include "mcu_periph/spi.h"
#endif
+#ifdef USE_DAC
+#include "mcu_periph/dac.h"
+#endif
#endif /* PERIPHERALS_AUTO_INIT */
void mcu_init(void) {
@@ -98,6 +101,9 @@ void mcu_init(void) {
#ifdef USE_SPI
spi_init();
#endif
+#ifdef USE_DAC
+ dac_init();
+#endif
#endif /* PERIPHERALS_AUTO_INIT */
}
diff --git a/sw/airborne/mcu_periph/dac.h b/sw/airborne/mcu_periph/dac.h
new file mode 100644
index 0000000000..f1a7d23b41
--- /dev/null
+++ b/sw/airborne/mcu_periph/dac.h
@@ -0,0 +1,9 @@
+#ifndef MCU_PERIPH_DAC_H
+#define MCU_PERIPH_DAC_H
+
+#include "mcu_periph/dac_arch.h"
+
+extern void dac_init(void);
+
+
+#endif /* MCU_PERIPH_DAC_H */
diff --git a/sw/airborne/mcu_periph/i2c.h b/sw/airborne/mcu_periph/i2c.h
index 43753823ce..93bd0f77ba 100644
--- a/sw/airborne/mcu_periph/i2c.h
+++ b/sw/airborne/mcu_periph/i2c.h
@@ -43,7 +43,6 @@ struct i2c_transaction {
uint8_t slave_addr;
uint16_t len_r;
uint8_t len_w;
- bool_t stop_after_transmit;
volatile uint8_t buf[I2C_BUF_LEN];
volatile enum I2CTransactionStatus status;
};
@@ -61,6 +60,10 @@ struct i2c_periph {
volatile enum I2CStatus status;
volatile uint8_t idx_buf;
void* reg_addr;
+ void *init_struct;
+ uint16_t scl_pin;
+ uint16_t sda_pin;
+ struct i2c_errors *errors;
};
@@ -132,40 +135,28 @@ extern bool_t i2c_idle(struct i2c_periph* p);
extern bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t);
#define I2CReceive(_p, _t, _s_addr, _len) { \
- _t.type = I2CTransRx; \
- _t.slave_addr = _s_addr; \
- _t.len_r = _len; \
- _t.len_w = 0; \
- _t.stop_after_transmit = TRUE; \
- i2c_submit(&(_p),&(_t)); \
- }
+ _t.type = I2CTransRx; \
+ _t.slave_addr = _s_addr; \
+ _t.len_r = _len; \
+ _t.len_w = 0; \
+ i2c_submit(&(_p),&(_t)); \
+}
#define I2CTransmit(_p, _t, _s_addr, _len) { \
- _t.type = I2CTransTx; \
- _t.slave_addr = _s_addr; \
- _t.len_r = 0; \
- _t.len_w = _len; \
- _t.stop_after_transmit = TRUE; \
- i2c_submit(&(_p),&(_t)); \
- }
+ _t.type = I2CTransTx; \
+ _t.slave_addr = _s_addr; \
+ _t.len_r = 0; \
+ _t.len_w = _len; \
+ i2c_submit(&(_p),&(_t)); \
+}
-#define I2CTransmitNoStop(_p, _t, _s_addr, _len) { \
- _t.type = I2CTransTx; \
- _t.slave_addr = _s_addr; \
- _t.len_r = 0; \
- _t.len_w = _len; \
- _t.stop_after_transmit = FALSE; \
- i2c_submit(&(_p),&(_t)); \
- }
-
-#define I2CTransceive(_p, _t, _s_addr, _len_w, _len_r) { \
- _t.type = I2CTransTxRx; \
- _t.slave_addr = _s_addr; \
- _t.len_r = _len_r; \
- _t.len_w = _len_w; \
- _t.stop_after_transmit = TRUE; \
- i2c_submit(&(_p),&(_t)); \
- }
+#define I2CTransceive(_p, _t, _s_addr, _len_w, _len_r) { \
+ _t.type = I2CTransTxRx; \
+ _t.slave_addr = _s_addr; \
+ _t.len_r = _len_r; \
+ _t.len_w = _len_w; \
+ i2c_submit(&(_p),&(_t)); \
+}
#endif /* I2C_H */
diff --git a/sw/airborne/mcu_periph/uart.h b/sw/airborne/mcu_periph/uart.h
index 96a8bb9bb8..eaaee1fcd4 100644
--- a/sw/airborne/mcu_periph/uart.h
+++ b/sw/airborne/mcu_periph/uart.h
@@ -155,4 +155,25 @@ extern void uart3_init(void);
#endif // USE_UART3
-#endif /* UART_H */
+#ifdef USE_UART5
+
+//TODO adapt to new driver
+extern void uart5_init( void );
+extern void uart5_transmit( uint8_t data );
+extern bool_t uart5_check_free_space( uint8_t len);
+
+#define Uart5Init uart5_init
+#define Uart5CheckFreeSpace(_x) uart5_check_free_space(_x)
+#define Uart5Transmit(_x) uart5_transmit(_x)
+#define Uart5SendMessage() {}
+
+#define UART5Init Uart5Init
+#define UART5CheckFreeSpace Uart5CheckFreeSpace
+#define UART5Transmit Uart5Transmit
+#define UART5SendMessage Uart5SendMessage
+#define UART5ChAvailable Uart5ChAvailable
+#define UART5Getch Uart5Getch
+
+#endif /* USE_UART5 */
+
+#endif /* MCU_PERIPH_UART_H */
diff --git a/sw/airborne/modules/airborne_ant_track/airborne_ant_track.c b/sw/airborne/modules/airborne_ant_track/airborne_ant_track.c
new file mode 100755
index 0000000000..1e6ddb8dd0
--- /dev/null
+++ b/sw/airborne/modules/airborne_ant_track/airborne_ant_track.c
@@ -0,0 +1,177 @@
+/*
+ * Determines antenna pan angle.
+ *
+ * project: Paparazzi
+ * description: Determines antenna pan angle from
+ * plane's and home's positions and plane's heading angle.
+ * Software might be optimized
+ * by removing multiplications with 0, it is left this
+ * way for better understandabilty and changeability.
+ *
+ * authors: Arnold Schroeter, Martin Mueller, Chris Efstathiou
+ *
+ *
+ *
+ *
+ */
+
+#if defined(USE_AIRBORNE_ANT_TRACKING) && USE_AIRBORNE_ANT_TRACKING == 1
+
+#include
+#include
+#include "inter_mcu.h"
+#include "subsystems/navigation/common_nav.h"
+#include "autopilot.h"
+#include "generated/flight_plan.h"
+#include "estimator.h"
+#include "subsystems/navigation/traffic_info.h"
+#include "airborne_ant_track.h"
+
+
+typedef struct {
+ float fx;
+ float fy;
+ float fz;} VECTOR;
+
+typedef struct {
+ float fx1; float fx2; float fx3;
+ float fy1; float fy2; float fy3;
+ float fz1; float fz2; float fz3;} MATRIX;
+
+float airborne_ant_pan;
+static bool_t ant_pan_positive = 0;
+
+void ant_point(void);
+static void vSubtractVectors(VECTOR* svA, VECTOR svB, VECTOR svC);
+static void vMultiplyMatrixByVector(VECTOR* svA, MATRIX smB, VECTOR svC);
+
+/*******************************************************************
+; function name: vSubtractVectors
+; description: subtracts two vectors a = b - c
+; parameters:
+;*******************************************************************/
+static void vSubtractVectors(VECTOR* svA, VECTOR svB, VECTOR svC)
+{
+ svA->fx = svB.fx - svC.fx;
+ svA->fy = svB.fy - svC.fy;
+ svA->fz = svB.fz - svC.fz;
+}
+
+/*******************************************************************
+; function name: vMultiplyMatrixByVector
+; description: multiplies matrix by vector svA = smB * svC
+; parameters:
+;*******************************************************************/
+static void vMultiplyMatrixByVector(VECTOR* svA, MATRIX smB, VECTOR svC)
+{
+ svA->fx = smB.fx1 * svC.fx + smB.fx2 * svC.fy + smB.fx3 * svC.fz;
+ svA->fy = smB.fy1 * svC.fx + smB.fy2 * svC.fy + smB.fy3 * svC.fz;
+ svA->fz = smB.fz1 * svC.fx + smB.fz2 * svC.fy + smB.fz3 * svC.fz;
+}
+
+void airborne_ant_point_init(void){
+
+return;
+}
+
+void airborne_ant_point_periodic(void)
+{
+float airborne_ant_pan_servo = 0;
+
+ static VECTOR svPlanePosition,
+ Home_Position,
+ Home_PositionForPlane,
+ Home_PositionForPlane2;
+
+ static MATRIX smRotation;
+
+ svPlanePosition.fx = estimator_y;
+ svPlanePosition.fy = estimator_x;
+ svPlanePosition.fz = estimator_z;
+
+ Home_Position.fx = waypoints[WP_HOME].y;
+ Home_Position.fy = waypoints[WP_HOME].x;
+ Home_Position.fz = waypoints[WP_HOME].a;
+
+ /* distance between plane and object */
+ vSubtractVectors(&Home_PositionForPlane, Home_Position, svPlanePosition);
+
+ /* yaw */
+ smRotation.fx1 = (float)(cos(estimator_hspeed_dir));
+ smRotation.fx2 = (float)(sin(estimator_hspeed_dir));
+ smRotation.fx3 = 0.;
+ smRotation.fy1 = -smRotation.fx2;
+ smRotation.fy2 = smRotation.fx1;
+ smRotation.fy3 = 0.;
+ smRotation.fz1 = 0.;
+ smRotation.fz2 = 0.;
+ smRotation.fz3 = 1.;
+
+ vMultiplyMatrixByVector(&Home_PositionForPlane2, smRotation, Home_PositionForPlane);
+
+
+/*
+ * This is for one axis pan antenna mechanisms. The default is to
+ * circle clockwise so view is right. The pan servo neutral makes
+ * the antenna look to the right with 0° given, 90° is to the back and
+ * -90° is to the front.
+ *
+ *
+ *
+ * plane front
+ *
+ * 90
+ ^
+ * I
+ * 135 I 45°
+ * \ I /
+ * \I/
+ * 180-------I------- 0°
+ * /I\
+ * / I \
+ * -135 I -45°
+ * I
+ * -90
+ * plane back
+ *
+ *
+ */
+
+ /* fPan = 0 -> antenna looks along the wing
+ 90 -> antenna looks in flight direction
+ -90 -> antenna looks backwards
+ */
+ /* fixed to the plane*/
+ airborne_ant_pan = (float)(atan2(Home_PositionForPlane2.fx, (Home_PositionForPlane2.fy)));
+
+ // I need to avoid oscillations around the 180 degree mark.
+ if (airborne_ant_pan > 0 && airborne_ant_pan <= RadOfDeg(175)){ ant_pan_positive = 1; }
+ if (airborne_ant_pan < 0 && airborne_ant_pan >= RadOfDeg(-175)){ ant_pan_positive = 0; }
+
+ if (airborne_ant_pan > RadOfDeg(175) && ant_pan_positive == 0){
+ airborne_ant_pan = RadOfDeg(-180);
+
+ }else if (airborne_ant_pan < RadOfDeg(-175) && ant_pan_positive){
+ airborne_ant_pan = RadOfDeg(180);
+ ant_pan_positive = 0;
+ }
+
+#ifdef ANT_PAN_NEUTRAL
+ airborne_ant_pan = airborne_ant_pan - RadOfDeg(ANT_PAN_NEUTRAL);
+ if (airborne_ant_pan > 0)
+ airborne_ant_pan_servo = MAX_PPRZ * (airborne_ant_pan / (RadOfDeg(ANT_PAN_MAX - ANT_PAN_NEUTRAL)));
+ else
+ airborne_ant_pan_servo = MIN_PPRZ * (airborne_ant_pan / (RadOfDeg(ANT_PAN_MIN - ANT_PAN_NEUTRAL)));
+#endif
+
+ airborne_ant_pan_servo = TRIM_PPRZ(airborne_ant_pan_servo);
+
+#ifdef COMMAND_ANT_PAN
+ ap_state->commands[COMMAND_ANT_PAN] = airborne_ant_pan_servo;
+#endif
+
+
+return;
+}
+
+#endif
diff --git a/sw/airborne/booz/arch/lpc21/booz2_analog_hw.h b/sw/airborne/modules/airborne_ant_track/airborne_ant_track.h
old mode 100644
new mode 100755
similarity index 62%
rename from sw/airborne/booz/arch/lpc21/booz2_analog_hw.h
rename to sw/airborne/modules/airborne_ant_track/airborne_ant_track.h
index ae87ad0c84..64cfaf50e6
--- a/sw/airborne/booz/arch/lpc21/booz2_analog_hw.h
+++ b/sw/airborne/modules/airborne_ant_track/airborne_ant_track.h
@@ -1,7 +1,7 @@
/*
- * $Id$
+ * $Id: point.h 2304 2008-02-07 21:35:08Z mmm $
*
- * Copyright (C) 2008-2009 Antoine Drouin
+ * Copyright (C) 2005-2008 Arnold Schroeter
*
* This file is part of paparazzi.
*
@@ -19,18 +19,16 @@
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
+ *
*/
-#ifndef BOOZ2_ANALOG_HW_H
-#define BOOZ2_ANALOG_HW_H
+#if defined(USE_AIRBORNE_ANT_TRACKING) && USE_AIRBORNE_ANT_TRACKING == 1
+#ifndef AIRBORNE_ANT_TRACK_H
+#define AIRBORNE_ANT_TRACK_H
-#include "LPC21xx.h"
-#include "std.h"
+extern float airborne_ant_pan;
+void airborne_ant_point_init(void);
+void airborne_ant_point_periodic(void);
-static inline void Booz2AnalogSetDAC(uint16_t x) {
- DACR = x << 6;
-}
-
-extern void booz2_analog_init_hw(void);
-
-#endif /* BOOZ2_ANALOG_HW_H */
+#endif /* AIRBORNE_ANT_TRACK_H */
+#endif // #if defined(USE_AIRBORNE_ANT_TRACKING) && USE_AIRBORNE_ANT_TRACKING == 1
diff --git a/sw/airborne/modules/ins/ins_arduimu_basic.c b/sw/airborne/modules/ins/ins_arduimu_basic.c
new file mode 100644
index 0000000000..18b9fe654b
--- /dev/null
+++ b/sw/airborne/modules/ins/ins_arduimu_basic.c
@@ -0,0 +1,147 @@
+/*
+C Datei für die Einbindung eines ArduIMU's
+Autoren@ZHAW: schmiemi
+ chaneren
+*/
+
+
+#include
+#include "modules/ins/ins_arduimu_basic.h"
+//#include "firmwares/fixedwing/main_fbw.h"
+#include "mcu_periph/i2c.h"
+
+// test
+#include "estimator.h"
+
+// für das Senden von GPS-Daten an den ArduIMU
+#include "gps.h"
+
+#define NB_DATA 9
+
+#ifndef ARDUIMU_I2C_DEV
+#define ARDUIMU_I2C_DEV i2c0
+#endif
+
+// Adresse des I2C Slaves: 0001 0110 letztes Bit ist für Read/Write
+// einzugebende Adresse im ArduIMU ist 0000 1011
+//da ArduIMU das Read/Write Bit selber anfügt.
+#define ArduIMU_SLAVE_ADDR 0x22
+
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+
+#include "mcu_periph/uart.h"
+#include "messages.h"
+#include "downlink.h"
+
+struct i2c_transaction ardu_gps_trans;
+struct i2c_transaction ardu_ins_trans;
+
+static int16_t recievedData[NB_DATA];
+
+struct FloatEulers arduimu_eulers;
+struct FloatRates arduimu_rates;
+struct FloatVect3 arduimu_accel;
+
+float ins_roll_neutral;
+float ins_pitch_neutral;
+
+void ArduIMU_init( void ) {
+ FLOAT_EULERS_ZERO(arduimu_eulers);
+ FLOAT_RATES_ZERO(arduimu_rates);
+ FLOAT_VECT3_ZERO(arduimu_accel);
+
+ ardu_ins_trans.status = I2CTransDone;
+ ardu_gps_trans.status = I2CTransDone;
+
+ ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
+ ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
+}
+
+#define FillBufWith32bit(_buf, _index, _value) { \
+ _buf[_index] = (uint8_t) (_value); \
+ _buf[_index+1] = (uint8_t) ((_value) >> 8); \
+ _buf[_index+2] = (uint8_t) ((_value) >> 16); \
+ _buf[_index+3] = (uint8_t) ((_value) >> 24); \
+}
+
+void ArduIMU_periodicGPS( void ) {
+
+ if (ardu_gps_trans.status != I2CTransDone) { return; }
+
+ FillBufWith32bit(ardu_gps_trans.buf, 0, (int32_t)gps_speed_3d); // speed 3D
+ FillBufWith32bit(ardu_gps_trans.buf, 4, (int32_t)gps_gspeed); // ground speed
+ FillBufWith32bit(ardu_gps_trans.buf, 8, (int32_t)gps_course); // course
+ ardu_gps_trans.buf[12] = gps_mode; // status gps fix
+ ardu_gps_trans.buf[13] = gps_status_flags; // status flags
+ I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 14);
+
+}
+
+void ArduIMU_periodic( void ) {
+ //Frequence defined in conf/modules/ins_arduimu.xml
+
+ if (ardu_ins_trans.status == I2CTransDone) {
+ I2CReceive(ARDUIMU_I2C_DEV, ardu_ins_trans, ArduIMU_SLAVE_ADDR, NB_DATA*2);
+ }
+
+}
+
+#include "math/pprz_algebra_int.h"
+/*
+ Buffer O: Roll
+ Buffer 1: Pitch
+ Buffer 2: Yaw
+ Buffer 3: Gyro X
+ Buffer 4: Gyro Y
+ Buffer 5: Gyro Z
+ Buffer 6: Accel X
+ Buffer 7: Accel Y
+ Buffer 8: Accel Z
+ */
+
+void ArduIMU_event( void ) {
+ // Handle INS I2C event
+ if (ardu_ins_trans.status == I2CTransSuccess) {
+ // received data from I2C transaction
+ recievedData[0] = (ardu_ins_trans.buf[1]<<8) | ardu_ins_trans.buf[0];
+ recievedData[1] = (ardu_ins_trans.buf[3]<<8) | ardu_ins_trans.buf[2];
+ recievedData[2] = (ardu_ins_trans.buf[5]<<8) | ardu_ins_trans.buf[4];
+ recievedData[3] = (ardu_ins_trans.buf[7]<<8) | ardu_ins_trans.buf[6];
+ recievedData[4] = (ardu_ins_trans.buf[9]<<8) | ardu_ins_trans.buf[8];
+ recievedData[5] = (ardu_ins_trans.buf[11]<<8) | ardu_ins_trans.buf[10];
+ recievedData[6] = (ardu_ins_trans.buf[13]<<8) | ardu_ins_trans.buf[12];
+ recievedData[7] = (ardu_ins_trans.buf[15]<<8) | ardu_ins_trans.buf[14];
+ recievedData[8] = (ardu_ins_trans.buf[17]<<8) | ardu_ins_trans.buf[16];
+
+ // Update ArduIMU data
+ arduimu_eulers.phi = ANGLE_FLOAT_OF_BFP(recievedData[0]);
+ arduimu_eulers.theta = ANGLE_FLOAT_OF_BFP(recievedData[1]);
+ arduimu_eulers.psi = ANGLE_FLOAT_OF_BFP(recievedData[2]);
+ arduimu_rates.p = RATE_FLOAT_OF_BFP(recievedData[3]);
+ arduimu_rates.q = RATE_FLOAT_OF_BFP(recievedData[4]);
+ arduimu_rates.r = RATE_FLOAT_OF_BFP(recievedData[5]);
+ arduimu_accel.x = ACCEL_FLOAT_OF_BFP(recievedData[6]);
+ arduimu_accel.y = ACCEL_FLOAT_OF_BFP(recievedData[7]);
+ arduimu_accel.z = ACCEL_FLOAT_OF_BFP(recievedData[8]);
+
+ // Update estimator
+ estimator_phi = arduimu_eulers.phi - ins_roll_neutral;
+ estimator_theta = arduimu_eulers.theta - ins_pitch_neutral;
+ estimator_p = arduimu_rates.p;
+ ardu_ins_trans.status = I2CTransDone;
+
+ //RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &arduimu_eulers.phi, &arduimu_eulers.theta, &arduimu_eulers.psi));
+ RunOnceEvery(15, DOWNLINK_SEND_IMU_GYRO(DefaultChannel, &arduimu_rates.p, &arduimu_rates.q, &arduimu_rates.r));
+ RunOnceEvery(15, DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, &arduimu_accel.x, &arduimu_accel.y, &arduimu_accel.z));
+ }
+ else if (ardu_ins_trans.status == I2CTransFailed) {
+ ardu_ins_trans.status = I2CTransDone;
+ }
+ // Handle GPS I2C event
+ if (ardu_gps_trans.status == I2CTransSuccess || ardu_gps_trans.status == I2CTransFailed) {
+ ardu_gps_trans.status = I2CTransDone;
+ }
+}
+
diff --git a/sw/airborne/modules/ins/ins_arduimu_basic.h b/sw/airborne/modules/ins/ins_arduimu_basic.h
new file mode 100644
index 0000000000..3e64cf3125
--- /dev/null
+++ b/sw/airborne/modules/ins/ins_arduimu_basic.h
@@ -0,0 +1,19 @@
+#ifndef ArduIMU_H
+#define ArduIMU_H
+
+#include
+#include "math/pprz_algebra_float.h"
+
+extern struct FloatEulers arduimu_eulers;
+extern struct FloatRates arduimu_rates;
+extern struct FloatVect3 arduimu_accel;
+
+extern float ins_roll_neutral;
+extern float ins_pitch_neutral;
+
+void ArduIMU_init( void );
+void ArduIMU_periodic( void );
+void ArduIMU_periodicGPS( void );
+void ArduIMU_event( void );
+
+#endif // ArduIMU_H
diff --git a/sw/airborne/modules/sensors/pressure_board_navarro.c b/sw/airborne/modules/sensors/pressure_board_navarro.c
index 987dc09f44..63e57f2d25 100644
--- a/sw/airborne/modules/sensors/pressure_board_navarro.c
+++ b/sw/airborne/modules/sensors/pressure_board_navarro.c
@@ -46,6 +46,15 @@
/* Weight for offset IIR filter */
#define PBN_OFFSET_FILTER 7
+/* Quadratic scale factor for airspeed */
+#ifndef PBN_AIRSPEED_SCALE
+#define PBN_AIRSPEED_SCALE (1./0.54)
+#endif
+
+/* Linear scale factor for altitude */
+#ifndef PBN_ALTITUDE_SCALE
+#define PBN_ALTITUDE_SCALE 0.32
+#endif
// Global variables
uint16_t altitude_adc;
@@ -61,6 +70,7 @@ uint16_t altitude_offset;
uint16_t airspeed_offset;
float pbn_altitude;
float pbn_airspeed;
+float airspeed_filter;
uint16_t startup_delay;
void pbn_init( void ) {
@@ -73,6 +83,7 @@ void pbn_init( void ) {
offset_cnt = OFFSET_NBSAMPLES_AVRG;
pbn_airspeed = 0.;
pbn_altitude = 0.;
+ airspeed_filter = PBN_OFFSET_FILTER;
}
@@ -106,20 +117,31 @@ void pbn_read_event( void ) {
if (offset_cnt > 0) {
// IIR filter to compute an initial offset
+#ifndef PBN_AIRSPEED_OFFSET
airspeed_offset = (PBN_OFFSET_FILTER * airspeed_offset + airspeed_adc) / (PBN_OFFSET_FILTER + 1);
+#else
+ airspeed_offset = PBN_AIRSPEED_OFFSET;
+#endif
+#ifndef PBN_ALTITUDE_OFFSET
altitude_offset = (PBN_OFFSET_FILTER * altitude_offset + altitude_adc) / (PBN_OFFSET_FILTER + 1);
+#else
+ altitude_offset = PBN_ALTITUDE_OFFSET;
+#endif
// decrease init counter
--offset_cnt;
}
else {
// Compute airspeed and altitude
- pbn_airspeed = (-4.45 + sqrtf(19.84-0.57*(float)(airspeed_offset-airspeed_adc)))/0.28;
- pbn_altitude = 0.32*(float)(altitude_adc-altitude_offset);
+ //pbn_airspeed = (-4.45 + sqrtf(19.84-0.57*(float)(airspeed_offset-airspeed_adc)))/0.28;
+ uint16_t diff = Max(airspeed_adc-airspeed_offset, 0);
+ float tmp_airspeed = sqrtf((float)diff * PBN_AIRSPEED_SCALE);
+ pbn_altitude = PBN_ALTITUDE_SCALE*(float)(altitude_adc-altitude_offset);
- //estimator_airspeed = (7*estimator_airspeed + pbn_airspeed ) / 8;
- //estimator_airspeed = Max(estimator_airspeed, 0.);
- //EstimatorSetAirspeed(pbn_airspeed);
+ pbn_airspeed = (airspeed_filter*pbn_airspeed + tmp_airspeed) / (airspeed_filter + 1.);
+#ifdef USE_AIRSPEED
+ EstimatorSetAirspeed(pbn_airspeed);
+#endif
//alt_kalman(pbn_altitude);
}
diff --git a/sw/airborne/modules/sensors/pressure_board_navarro.h b/sw/airborne/modules/sensors/pressure_board_navarro.h
index 2bd4edba89..5b1fe68bff 100644
--- a/sw/airborne/modules/sensors/pressure_board_navarro.h
+++ b/sw/airborne/modules/sensors/pressure_board_navarro.h
@@ -43,6 +43,7 @@ extern uint16_t airspeed_adc;
extern uint16_t altitude_offset;
extern uint16_t airspeed_offset;
extern float pbn_altitude, pbn_airspeed;
+extern float airspeed_filter;
extern bool_t data_valid;
extern struct i2c_transaction pbn_trans;
diff --git a/sw/airborne/modules/sonar/sonar_maxbotix_booz.c b/sw/airborne/modules/sonar/sonar_maxbotix.c
similarity index 81%
rename from sw/airborne/modules/sonar/sonar_maxbotix_booz.c
rename to sw/airborne/modules/sonar/sonar_maxbotix.c
index fad7db7e8d..c339212cea 100644
--- a/sw/airborne/modules/sonar/sonar_maxbotix_booz.c
+++ b/sw/airborne/modules/sonar/sonar_maxbotix.c
@@ -1,5 +1,4 @@
/*
- * $Id: demo_module.c 3079 2009-03-11 16:55:42Z gautier $
*
* Copyright (C) 2010 Gautier Hattenberger
*
@@ -22,22 +21,25 @@
*
*/
-#include "sonar_maxbotix_booz.h"
-#include "booz2_analog.h"
+#include "modules/sonar/sonar_maxbotix.h"
+#include "mcu_periph/adc.h"
uint16_t sonar_meas;
bool_t sonar_data_available;
+static struct adc_buf sonar_adc;
+
void maxbotix_init(void) {
sonar_meas = 0;
sonar_data_available = FALSE;
+
+ adc_buf_channel(ADC_CHANNEL_SONAR, &sonar_adc, DEFAULT_AV_NB_SAMPLE);
}
/** Read ADC value to update sonar measurement
*/
void maxbotix_read(void) {
- booz2_analog_extra_adc_read();
- sonar_meas = booz2_adc_1;
+ sonar_meas = sonar_adc.sum / sonar_adc.av_nb_sample;
sonar_data_available = TRUE;
}
diff --git a/sw/airborne/modules/sonar/sonar_maxbotix_booz.h b/sw/airborne/modules/sonar/sonar_maxbotix.h
similarity index 84%
rename from sw/airborne/modules/sonar/sonar_maxbotix_booz.h
rename to sw/airborne/modules/sonar/sonar_maxbotix.h
index 596a8e5a75..b692f0889a 100644
--- a/sw/airborne/modules/sonar/sonar_maxbotix_booz.h
+++ b/sw/airborne/modules/sonar/sonar_maxbotix.h
@@ -1,5 +1,4 @@
/*
- * $Id: demo_module.h 3079 2009-03-11 16:55:42Z gautier $
*
* Copyright (C) 2010 Gautier Hattenberger
*
@@ -22,9 +21,9 @@
*
*/
-/** \file sonar_maxbotix_booz.h
+/** \file sonar_maxbotix.h
*
- * simple driver to deal with one maxbotix sensor on booz AP
+ * simple driver to deal with one maxbotix sensor
*/
#ifndef SONAR_MAXBOTIX_BOOZ_H
@@ -39,7 +38,7 @@ extern bool_t sonar_data_available;
extern void maxbotix_init(void);
extern void maxbotix_read(void);
-#include "subsystems/ins.h" // needed because ins is not a module
+//#include "subsystems/ins.h" // needed because ins is not a module
#define SonarEvent(_handler) { \
if (sonar_data_available) { \
diff --git a/sw/airborne/peripherals/ami601.c b/sw/airborne/peripherals/ami601.c
index 81b1e512a2..1f7f3d4c76 100644
--- a/sw/airborne/peripherals/ami601.c
+++ b/sw/airborne/peripherals/ami601.c
@@ -17,7 +17,6 @@ void ami601_init( void ) {
}
ami601_i2c_trans.status = I2CTransSuccess;
ami601_i2c_trans.slave_addr = AMI601_SLAVE_ADDR;
- ami601_i2c_trans.stop_after_transmit = TRUE;
ami601_nb_err = 0;
ami601_status = AMI601_IDLE;
@@ -26,7 +25,7 @@ void ami601_init( void ) {
void ami601_read( void ) {
if (ami601_status != AMI601_IDLE) {
ami601_nb_err++;
- ami601_status == AMI601_IDLE;
+ ami601_status = AMI601_IDLE;
}
else {
ami601_status = AMI601_SENDING_REQ;
diff --git a/sw/airborne/peripherals/hmc5843.c b/sw/airborne/peripherals/hmc5843.c
index 3f5c63f9f3..f02c148312 100644
--- a/sw/airborne/peripherals/hmc5843.c
+++ b/sw/airborne/peripherals/hmc5843.c
@@ -11,7 +11,6 @@ void hmc5843_init(void)
{
hmc5843.i2c_trans.status = I2CTransSuccess;
hmc5843.i2c_trans.slave_addr = HMC5843_ADDR;
- hmc5843.i2c_trans.stop_after_transmit = TRUE;
hmc5843_arch_init();
}
@@ -46,11 +45,11 @@ void hmc5843_idle_task(void)
{
if (hmc5843.initialized && hmc5843.ready_for_read && (hmc5843.i2c_trans.status == I2CTransSuccess || hmc5843.i2c_trans.status == I2CTransFailed)) {
if (i2c2.status == I2CIdle && i2c_idle(&i2c2)) {
+ hmc5843.ready_for_read = FALSE;
hmc5843.i2c_trans.type = I2CTransRx;
hmc5843.i2c_trans.len_r = 7;
i2c_submit(&i2c2, &hmc5843.i2c_trans);
hmc5843.reading = TRUE;
- hmc5843.ready_for_read = FALSE;
}
}
@@ -70,8 +69,10 @@ void hmc5843_periodic(void)
if (!hmc5843.initialized) {
send_config();
hmc5843.initialized = TRUE;
- } else if (hmc5843.timeout++ > HMC5843_TIMEOUT) {
+ } else if (hmc5843.timeout++ > HMC5843_TIMEOUT && i2c2.status == I2CIdle && i2c_idle(&i2c2)){
+#ifdef USE_HMC59843_ARCH_RESET
hmc5843_arch_reset();
+#endif
hmc5843.i2c_trans.type = I2CTransRx;
hmc5843.i2c_trans.len_r = 7;
i2c_submit(&i2c2, &hmc5843.i2c_trans);
diff --git a/sw/airborne/peripherals/ms2001.c b/sw/airborne/peripherals/ms2100.c
similarity index 74%
rename from sw/airborne/peripherals/ms2001.c
rename to sw/airborne/peripherals/ms2100.c
index 310efe2f83..1e0940339a 100644
--- a/sw/airborne/peripherals/ms2001.c
+++ b/sw/airborne/peripherals/ms2100.c
@@ -21,21 +21,21 @@
* Boston, MA 02111-1307, USA.
*/
-#include "ms2001.h"
+#include "ms2100.h"
-volatile uint8_t ms2001_status;
-volatile int16_t ms2001_values[MS2001_NB_AXIS];
+volatile uint8_t ms2100_status;
+volatile int16_t ms2100_values[MS2100_NB_AXIS];
-void ms2001_init( void ) {
+void ms2100_init( void ) {
- ms2001_arch_init();
+ ms2100_arch_init();
uint8_t i;
- for (i=0; i
#include "../../test/pprz_algebra_print.h"
+void ahrs_update_mag_full(void);
+void ahrs_update_mag_2d(void);
+void ahrs_update_mag_2d_dumb(void);
static inline void compute_imu_quat_and_euler_from_rmat(void);
+static inline void compute_imu_rmat_and_euler_from_quat(void);
static inline void compute_body_orientation_and_rates(void);
@@ -45,25 +48,23 @@ struct AhrsFloatCmplRmat ahrs_impl;
void ahrs_init(void) {
ahrs_float.status = AHRS_UNINIT;
- /*
- * Initialises our IMU alignement variables
- * This should probably done in the IMU code instead
- */
+ /* Initialises IMU alignement */
struct FloatEulers body_to_imu_euler =
{IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);
-
- /* set ltp_to_body to zero */
+
+ /* Set ltp_to_body to zero */
FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat);
FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler);
FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat);
FLOAT_RATES_ZERO(ahrs_float.body_rate);
- /* set ltp_to_imu so that body is zero */
+ /* Set ltp_to_imu so that body is zero */
QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler);
+
FLOAT_RATES_ZERO(ahrs_float.imu_rate);
}
@@ -95,30 +96,32 @@ void ahrs_propagate(void) {
struct FloatRates gyro_float;
RATES_FLOAT_OF_BFP(gyro_float, imu.gyro_prev);
/* unbias measurement */
- RATES_DIFF(ahrs_float.imu_rate, gyro_float, ahrs_impl.gyro_bias);
- const float dt = 1./512.;
+ RATES_SUB(gyro_float, ahrs_impl.gyro_bias);
+
+#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
+ const float alpha = 0.1;
+ FLOAT_RATES_LIN_CMB(ahrs_float.imu_rate, ahrs_float.imu_rate, (1.-alpha), gyro_float, alpha);
+#else
+ RATES_COPY(ahrs_float.imu_rate,gyro_float);
+#endif
+
/* add correction */
struct FloatRates omega;
- RATES_SUM(omega, ahrs_float.imu_rate, ahrs_impl.rate_correction);
- // DISPLAY_FLOAT_RATES("omega ", omega);
+ RATES_SUM(omega, gyro_float, ahrs_impl.rate_correction);
/* and zeros it */
FLOAT_RATES_ZERO(ahrs_impl.rate_correction);
- /* first order integration of rotation matrix */
- struct FloatRMat exp_omega_dt = {
- { 1. , dt*omega.r, -dt*omega.q,
- -dt*omega.r, 1. , dt*omega.p,
- dt*omega.q, -dt*omega.p, 1. }};
- struct FloatRMat R_tdt;
- FLOAT_RMAT_COMP(R_tdt, ahrs_float.ltp_to_imu_rmat, exp_omega_dt);
- memcpy(&ahrs_float.ltp_to_imu_rmat, &R_tdt, sizeof(R_tdt));
-
+ const float dt = 1./AHRS_PROPAGATE_FREQUENCY;
+#ifdef AHRS_PROPAGATE_RMAT
+ FLOAT_RMAT_INTEGRATE_FI(ahrs_float.ltp_to_imu_rmat, omega, dt );
float_rmat_reorthogonalize(&ahrs_float.ltp_to_imu_rmat);
- // struct FloatRMat test;
- // FLOAT_RMAT_COMP_INV(test, ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_rmat);
- // DISPLAY_FLOAT_RMAT("foo", test);
-
compute_imu_quat_and_euler_from_rmat();
+#endif
+#ifdef AHRS_PROPAGATE_QUAT
+ FLOAT_QUAT_INTEGRATE(ahrs_float.ltp_to_imu_quat, omega, dt);
+ FLOAT_QUAT_NORMALIZE(ahrs_float.ltp_to_imu_quat);
+ compute_imu_rmat_and_euler_from_quat();
+#endif
compute_body_orientation_and_rates();
}
@@ -127,23 +130,102 @@ void ahrs_update_accel(void) {
struct FloatVect3 accel_float;
ACCELS_FLOAT_OF_BFP(accel_float, imu.accel);
- struct FloatVect3* r2 = (struct FloatVect3*)(&RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 2,0));
+
+#ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN
+ float v = FLOAT_VECT3_NORM(ahrs_impl.est_ltp_speed);
+ accel_float.y -= v * ahrs_float.imu_rate.r;
+ accel_float.z -= -v * ahrs_float.imu_rate.q;
+#endif
+
+ struct FloatVect3 c2 = { RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 0,2),
+ RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 1,2),
+ RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 2,2)};
struct FloatVect3 residual;
- FLOAT_VECT3_CROSS_PRODUCT(residual, accel_float, (*r2));
+ FLOAT_VECT3_CROSS_PRODUCT(residual, accel_float, c2);
+#ifdef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
/* heuristic on acceleration norm */
const float acc_norm = FLOAT_VECT3_NORM(accel_float);
- const float weight = Chop(1.-2*fabs(1-acc_norm/9.81), 0., 1.);
- //const float weight = 1.;
+ const float weight = Chop(1.-6*fabs((9.81-acc_norm)/9.81), 0., 1.);
+#else
+ const float weight = 1.;
+#endif
/* compute correction */
- const float gravity_rate_update_gain = 5e-2;
+ const float gravity_rate_update_gain = -5e-2; // -5e-2
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, weight*gravity_rate_update_gain);
- const float gravity_bias_update_gain = -5e-6;
+#if 1
+ const float gravity_bias_update_gain = 1e-5; // -5e-6
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, weight*gravity_bias_update_gain);
+#else
+ const float alpha = 5e-4;
+ FLOAT_RATES_SCALE(ahrs_impl.gyro_bias, 1.-alpha);
+ FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, alpha);
+#endif
/* FIXME: saturate bias */
}
+
void ahrs_update_mag(void) {
+#ifdef AHRS_MAG_UPDATE_YAW_ONLY
+ ahrs_update_mag_2d();
+ // ahrs_update_mag_2d_dumb();
+#else
+ ahrs_update_mag_full();
+#endif
+}
+
+void ahrs_update_mag_full(void) {
+
+ const struct FloatVect3 expected_ltp = {AHRS_H_X, AHRS_H_Y, AHRS_H_Z};
+ struct FloatVect3 expected_imu;
+ FLOAT_RMAT_VECT3_MUL(expected_imu, ahrs_float.ltp_to_imu_rmat, expected_ltp);
+
+ struct FloatVect3 measured_imu;
+ MAGS_FLOAT_OF_BFP(measured_imu, imu.mag);
+
+ struct FloatVect3 residual_imu;
+ FLOAT_VECT3_CROSS_PRODUCT(residual_imu, measured_imu, expected_imu);
+ // DISPLAY_FLOAT_VECT3("# expected", expected_imu);
+ // DISPLAY_FLOAT_VECT3("# measured", measured_imu);
+ // DISPLAY_FLOAT_VECT3("# residual", residual);
+
+ const float mag_rate_update_gain = 2.5;
+ FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain);
+
+ const float mag_bias_update_gain = -2.5e-3;
+ FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain);
+
+}
+
+void ahrs_update_mag_2d(void) {
+
+ const struct FloatVect2 expected_ltp = {AHRS_H_X, AHRS_H_Y};
+
+ struct FloatVect3 measured_imu;
+ MAGS_FLOAT_OF_BFP(measured_imu, imu.mag);
+ struct FloatVect3 measured_ltp;
+ FLOAT_RMAT_VECT3_TRANSP_MUL(measured_ltp, ahrs_float.ltp_to_imu_rmat, measured_imu);
+
+ const struct FloatVect3 residual_ltp =
+ { 0,
+ 0,
+ measured_ltp.x * expected_ltp.y - measured_ltp.y * expected_ltp.x };
+
+ // printf("res : %f\n", residual_ltp.z);
+
+ struct FloatVect3 residual_imu;
+ FLOAT_RMAT_VECT3_MUL(residual_imu, ahrs_float.ltp_to_imu_rmat, residual_ltp);
+
+ const float mag_rate_update_gain = 2.5;
+ FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain);
+
+ const float mag_bias_update_gain = -2.5e-3;
+ FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain);
+
+}
+
+
+void ahrs_update_mag_full_2d_dumb(void) {
/* project mag on local tangeant plane */
struct FloatVect3 magf;
@@ -156,39 +238,19 @@ void ahrs_update_mag(void) {
const float me = 0. * magf.x + cphi *magf.y - sphi *magf.z;
const float res_norm = -RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 0,0)*me + RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 1,0)*mn;
- printf("res norm %f\n", res_norm);
struct FloatVect3* r2 = (struct FloatVect3*)(&RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 2,0));
- DISPLAY_FLOAT_VECT3("r2", (*r2));
const float mag_rate_update_gain = 2.5;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, (*r2), (mag_rate_update_gain*res_norm));
- DISPLAY_FLOAT_RATES("corr", ahrs_impl.rate_correction);
const float mag_bias_update_gain = -2.5e-4;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, (*r2), (mag_bias_update_gain*res_norm));
- /* FIXME: saturate bias */
}
-void ahrs_update_mag2(void) {
- const struct FloatVect3 expected_ltp = {AHRS_H_X, AHRS_H_Y, AHRS_H_Z};
- struct FloatVect3 expected_imu;
- FLOAT_RMAT_VECT3_MUL(expected_imu, ahrs_float.ltp_to_imu_rmat, expected_ltp);
- struct FloatVect3 measured_imu;
- MAGS_FLOAT_OF_BFP(measured_imu, imu.mag);
- struct FloatVect3 residual;
- FLOAT_VECT3_CROSS_PRODUCT(residual, measured_imu, expected_imu);
- // FLOAT_VECT3_DIFF(residual, expected_imu, measured_imu);
- DISPLAY_FLOAT_VECT3(" expected", expected_imu);
- DISPLAY_FLOAT_VECT3(" measured", measured_imu);
- DISPLAY_FLOAT_VECT3("residual", residual);
- const float mag_rate_update_gain = 2.5;
- FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, mag_rate_update_gain);
-
-}
/*
* Compute ltp to imu rotation in euler angles and quaternion representations
@@ -199,6 +261,14 @@ static inline void compute_imu_quat_and_euler_from_rmat(void) {
FLOAT_EULERS_OF_RMAT(ahrs_float.ltp_to_imu_euler, ahrs_float.ltp_to_imu_rmat);
}
+
+static inline void compute_imu_rmat_and_euler_from_quat(void) {
+ FLOAT_RMAT_OF_QUAT(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_quat);
+ FLOAT_EULERS_OF_RMAT(ahrs_float.ltp_to_imu_euler, ahrs_float.ltp_to_imu_rmat);
+}
+
+
+
/*
* Compute body orientation and rates from imu orientation and rates
*/
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.h b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.h
index ba5aaa553c..ecfa367ccf 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.h
@@ -27,6 +27,9 @@
struct AhrsFloatCmplRmat {
struct FloatRates gyro_bias;
struct FloatRates rate_correction;
+ /* for gravity correction during coordinated turns */
+ struct FloatVect3 est_ltp_speed;
+
/*
Holds float version of IMU alignement
in order to be able to run against the fixed point
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c b/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c
index 50e2c6f6df..f1e781a36a 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c
@@ -315,7 +315,7 @@ void ahrs_propagate(void) {
bafl_qnorm = FLOAT_QUAT_NORM(bafl_quat);
//TODO check if broot force normalization is good, use lagrange normalization?
- FLOAT_QUAT_NORMALISE(bafl_quat);
+ FLOAT_QUAT_NORMALIZE(bafl_quat);
/*
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_utils.h b/sw/airborne/subsystems/ahrs/ahrs_float_utils.h
index 679f0774b5..03374c8149 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_utils.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_utils.h
@@ -8,6 +8,7 @@ static inline void ahrs_float_get_euler_from_accel_mag(struct FloatEulers* e, st
const float phi = atan2f(-accelf.y, -accelf.z);
const float cphi = cosf(phi);
const float theta = atan2f(cphi*accelf.x, -accelf.z);
+
/* get psi from magnetometer */
/* project mag on local tangeant plane */
struct FloatVect3 magf;
@@ -17,8 +18,10 @@ static inline void ahrs_float_get_euler_from_accel_mag(struct FloatEulers* e, st
const float stheta = sinf(theta);
const float mn = ctheta * magf.x + sphi*stheta*magf.y + cphi*stheta*magf.z;
const float me = 0. * magf.x + cphi *magf.y - sphi *magf.z;
- const float psi = -atan2f(me, mn);
+ float psi = -atan2f(me, mn) + atan2(AHRS_H_Y, AHRS_H_X);
+ if (psi > M_PI) psi -= 2.*M_PI; if (psi < -M_PI) psi+= 2.*M_PI;
EULERS_ASSIGN(*e, phi, theta, psi);
+
}
#endif /* AHRS_FLOAT_UTILS_H */
diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c
new file mode 100644
index 0000000000..a74f631cb3
--- /dev/null
+++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c
@@ -0,0 +1,322 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2011 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.
+ */
+
+
+// TODO
+//
+// gravity heuristic
+// gps based gravity correction
+// gps update for yaw on fixed wing ?
+//
+
+#include "subsystems/ahrs/ahrs_int_cmpl.h"
+#include "subsystems/ahrs/ahrs_aligner.h"
+#include "subsystems/ahrs/ahrs_int_utils.h"
+
+#include "subsystems/imu.h"
+#include "math/pprz_trig_int.h"
+#include "math/pprz_algebra_int.h"
+
+#include "generated/airframe.h"
+
+//#include "../../test/pprz_algebra_print.h"
+
+static inline void ahrs_update_mag_full(void);
+static inline void ahrs_update_mag_2d(void);
+
+
+/* in place quaternion first order integration with constante rotational velocity */
+/* */
+#define INT32_QUAT_INTEGRATE_FI(_q, _hr, _omega, _f) { \
+ _hr.qi += -_omega.p*_q.qx - _omega.q*_q.qy - _omega.r*_q.qz; \
+ _hr.qx += _omega.p*_q.qi + _omega.r*_q.qy - _omega.q*_q.qz; \
+ _hr.qy += _omega.q*_q.qi - _omega.r*_q.qx + _omega.p*_q.qz; \
+ _hr.qz += _omega.r*_q.qi + _omega.q*_q.qx - _omega.p*_q.qy; \
+ \
+ ldiv_t _div = ldiv(_hr.qi, ((1<>INT32_ACCEL_FRAC) * ahrs.imu_rate.r)
+ >>(INT32_SPEED_FRAC+INT32_RATE_FRAC-INT32_ACCEL_FRAC-INT32_ACCEL_FRAC),
+ -((ahrs_impl.ltp_vel_norm>>INT32_ACCEL_FRAC) * ahrs.imu_rate.q)
+ >>(INT32_SPEED_FRAC+INT32_RATE_FRAC-INT32_ACCEL_FRAC-INT32_ACCEL_FRAC)
+ };
+ struct Int32Vect3 corrected_gravity;
+ VECT3_DIFF(corrected_gravity, imu.accel, Xdd_imu);
+ INT32_VECT3_CROSS_PRODUCT(residual, corrected_gravity, c2);
+#else
+ INT32_VECT3_CROSS_PRODUCT(residual, imu.accel, c2);
+#endif
+
+ // residual FRAC : ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24
+ // rate_correction FRAC = RATE_FRAC = 12
+ // 2^12 / 2^24 * 5e-2 = 1/81920
+ ahrs_impl.rate_correction.p += -residual.x/82000;
+ ahrs_impl.rate_correction.q += -residual.y/82000;
+ ahrs_impl.rate_correction.r += -residual.z/82000;
+
+ // residual FRAC = ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24
+ // high_rez_bias = RATE_FRAC+28 = 40
+ // 2^40 / 2^24 * 5e-6 = 1/3.05
+
+ // ahrs_impl.high_rez_bias.p += residual.x*3;
+ // ahrs_impl.high_rez_bias.q += residual.y*3;
+ // ahrs_impl.high_rez_bias.r += residual.z*3;
+
+ ahrs_impl.high_rez_bias.p += residual.x;
+ ahrs_impl.high_rez_bias.q += residual.y;
+ ahrs_impl.high_rez_bias.r += residual.z;
+
+
+ /* */
+ INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28);
+
+
+}
+
+void ahrs_update_mag(void) {
+#ifdef AHRS_MAG_UPDATE_YAW_ONLY
+ ahrs_update_mag_2d();
+#else
+ ahrs_update_mag_full();
+#endif
+}
+
+
+static inline void ahrs_update_mag_full(void) {
+ const struct Int32Vect3 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X),
+ MAG_BFP_OF_REAL(AHRS_H_Y),
+ MAG_BFP_OF_REAL(AHRS_H_Z)};
+ struct Int32Vect3 expected_imu;
+ INT32_RMAT_VMULT(expected_imu, ahrs.ltp_to_imu_rmat, expected_ltp);
+
+ struct Int32Vect3 residual;
+ INT32_VECT3_CROSS_PRODUCT(residual, imu.mag, expected_imu);
+
+ ahrs_impl.rate_correction.p += residual.x/32/16;
+ ahrs_impl.rate_correction.q += residual.y/32/16;
+ ahrs_impl.rate_correction.r += residual.z/32/16;
+
+
+ ahrs_impl.high_rez_bias.p += -residual.x/32*1024;
+ ahrs_impl.high_rez_bias.q += -residual.y/32*1024;
+ ahrs_impl.high_rez_bias.r += -residual.z/32*1024;
+
+
+ INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28);
+
+}
+
+
+static inline void ahrs_update_mag_2d(void) {
+
+ const struct Int32Vect2 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X),
+ MAG_BFP_OF_REAL(AHRS_H_Y)};
+
+ struct Int32Vect3 measured_ltp;
+ INT32_RMAT_TRANSP_VMULT(measured_ltp, ahrs.ltp_to_imu_rmat, imu.mag);
+
+ struct Int32Vect3 residual_ltp =
+ { 0,
+ 0,
+ (measured_ltp.x * expected_ltp.y - measured_ltp.y * expected_ltp.x)/(1<<5)};
+
+ struct Int32Vect3 residual_imu;
+ INT32_RMAT_VMULT(residual_imu, ahrs.ltp_to_imu_rmat, residual_ltp);
+
+ // residual_ltp FRAC = 2 * MAG_FRAC = 22
+ // rate_correction FRAC = RATE_FRAC = 12
+ // 2^12 / 2^22 * 2.5 = 1/410
+
+ // ahrs_impl.rate_correction.p += residual_imu.x*(1<<5)/410;
+ // ahrs_impl.rate_correction.q += residual_imu.y*(1<<5)/410;
+ // ahrs_impl.rate_correction.r += residual_imu.z*(1<<5)/410;
+
+ ahrs_impl.rate_correction.p += residual_imu.x/16;
+ ahrs_impl.rate_correction.q += residual_imu.y/16;
+ ahrs_impl.rate_correction.r += residual_imu.z/16;
+
+
+ // residual_ltp FRAC = 2 * MAG_FRAC = 22
+ // high_rez_bias = RATE_FRAC+28 = 40
+ // 2^40 / 2^22 * 2.5e-3 = 655
+
+ // ahrs_impl.high_rez_bias.p -= residual_imu.x*(1<<5)*655;
+ // ahrs_impl.high_rez_bias.q -= residual_imu.y*(1<<5)*655;
+ // ahrs_impl.high_rez_bias.r -= residual_imu.z*(1<<5)*655;
+
+ ahrs_impl.high_rez_bias.p -= residual_imu.x*1024;
+ ahrs_impl.high_rez_bias.q -= residual_imu.y*1024;
+ ahrs_impl.high_rez_bias.r -= residual_imu.z*1024;
+
+
+ INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28);
+
+}
+
+
+/* Compute ltp to imu rotation in quaternion and rotation matrice representation
+ from the euler angle representation */
+__attribute__ ((always_inline)) static inline void compute_imu_quat_and_rmat_from_euler(void) {
+
+ /* Compute LTP to IMU quaternion */
+ INT32_QUAT_OF_EULERS(ahrs.ltp_to_imu_quat, ahrs.ltp_to_imu_euler);
+ /* Compute LTP to IMU rotation matrix */
+ INT32_RMAT_OF_EULERS(ahrs.ltp_to_imu_rmat, ahrs.ltp_to_imu_euler);
+
+}
+
+/* Compute ltp to imu rotation in euler angles and rotation matrice representation
+ from the quaternion representation */
+__attribute__ ((always_inline)) static inline void compute_imu_euler_and_rmat_from_quat(void) {
+
+ /* Compute LTP to IMU euler */
+ INT32_EULERS_OF_QUAT(ahrs.ltp_to_imu_euler, ahrs.ltp_to_imu_quat);
+ /* Compute LTP to IMU rotation matrix */
+ INT32_RMAT_OF_QUAT(ahrs.ltp_to_imu_rmat, ahrs.ltp_to_imu_quat);
+
+}
+
+__attribute__ ((always_inline)) static inline void compute_body_orientation(void) {
+
+ /* Compute LTP to BODY quaternion */
+ INT32_QUAT_COMP_INV(ahrs.ltp_to_body_quat, ahrs.ltp_to_imu_quat, imu.body_to_imu_quat);
+ /* Compute LTP to BODY rotation matrix */
+ INT32_RMAT_COMP_INV(ahrs.ltp_to_body_rmat, ahrs.ltp_to_imu_rmat, imu.body_to_imu_rmat);
+ /* compute LTP to BODY eulers */
+ INT32_EULERS_OF_RMAT(ahrs.ltp_to_body_euler, ahrs.ltp_to_body_rmat);
+ /* compute body rates */
+ INT32_RMAT_TRANSP_RATEMULT(ahrs.body_rate, imu.body_to_imu_rmat, ahrs.imu_rate);
+
+}
diff --git a/sw/airborne/booz/booz2_analog.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h
similarity index 63%
rename from sw/airborne/booz/booz2_analog.c
rename to sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h
index 2e0f153b78..f1f833b656 100644
--- a/sw/airborne/booz/booz2_analog.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h
@@ -1,7 +1,7 @@
/*
* $Id$
*
- * Copyright (C) 2008-2009 Antoine Drouin
+ * Copyright (C) 2011 The Paparazzi Team
*
* This file is part of paparazzi.
*
@@ -21,29 +21,23 @@
* Boston, MA 02111-1307, USA.
*/
-#include "booz2_analog.h"
+#ifndef AHRS_INT_CMPL_H
+#define AHRS_INT_CMPL_H
+#include "subsystems/ahrs.h"
#include "std.h"
+#include "math/pprz_algebra_int.h"
-// battery on AD0.3 on P0.30
-// baro on AD0.1 on P0.28
-
-#define CHAN_BAT 3
-#define CHAN_BARO 1
-
-
-void booz2_analog_init( void ) {
-
- booz2_analog_init_hw();
-
-}
-
-#ifdef USE_EXTRA_ADC
-// Read manually baro (100Hz) and bat (10Hz)
-void booz2_analog_periodic( void ) {
- // baro
- RunOnceEvery(5,booz2_analog_baro_read());
- // bat
- RunOnceEvery(50,booz2_analog_bat_read());
-}
+struct AhrsIntCmpl {
+ struct Int32Rates gyro_bias;
+ struct Int32Rates rate_correction;
+ struct Int64Quat high_rez_quat;
+ struct Int64Rates high_rez_bias;
+#ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN
+ int32_t ltp_vel_norm;
#endif
+};
+
+extern struct AhrsIntCmpl ahrs_impl;
+
+#endif /* AHRS_INT_CMPL_H */
diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_utils.h b/sw/airborne/subsystems/ahrs/ahrs_int_utils.h
new file mode 100644
index 0000000000..84918fcc62
--- /dev/null
+++ b/sw/airborne/subsystems/ahrs/ahrs_int_utils.h
@@ -0,0 +1,47 @@
+#ifndef AHRS_INT_UTILS_H
+#define AHRS_INT_UTILS_H
+
+//#include "../../test/pprz_algebra_print.h"
+#include "math/pprz_algebra_int.h"
+
+#include "generated/airframe.h"
+
+static inline void ahrs_int_get_euler_from_accel_mag(struct Int32Eulers* e, struct Int32Vect3* accel, struct Int32Vect3* mag) {
+ // DISPLAY_INT32_VECT3("# accel", (*accel));
+ const float fphi = atan2f(-accel->y, -accel->z);
+ // printf("# atan float %f\n", DegOfRad(fphi));
+ e->phi = ANGLE_BFP_OF_REAL(fphi);
+
+ int32_t cphi;
+ PPRZ_ITRIG_COS(cphi, e->phi);
+ int32_t cphi_ax = -INT_MULT_RSHIFT(cphi, accel->x, INT32_TRIG_FRAC);
+ const float ftheta = atan2f(-cphi_ax, -accel->z);
+ e->theta = ANGLE_BFP_OF_REAL(ftheta);
+
+ int32_t sphi;
+ PPRZ_ITRIG_SIN(sphi, e->phi);
+ int32_t stheta;
+ PPRZ_ITRIG_SIN(stheta, e->theta);
+ int32_t ctheta;
+ PPRZ_ITRIG_COS(ctheta, e->theta);
+
+ int32_t sphi_stheta = (sphi*stheta)>>INT32_TRIG_FRAC;
+ int32_t cphi_stheta = (cphi*stheta)>>INT32_TRIG_FRAC;
+ //int32_t sphi_ctheta = (sphi*ctheta)>>INT32_TRIG_FRAC;
+ //int32_t cphi_ctheta = (cphi*ctheta)>>INT32_TRIG_FRAC;
+
+ const int32_t mn = ctheta * mag->x + sphi_stheta * mag->y + cphi_stheta * mag->z;
+ const int32_t me = 0 * mag->x + cphi * mag->y - sphi * mag->z;
+ //const int32_t md =
+ // -stheta * imu.mag.x +
+ // sphi_ctheta * imu.mag.y +
+ // cphi_ctheta * imu.mag.z;
+ // float m_psi = -atan2(me, mn);
+ const float mag_dec = atan2(-AHRS_H_Y, AHRS_H_X);
+ const float fpsi = atan2f(-me, mn) - mag_dec;
+ e->psi = ANGLE_BFP_OF_REAL(fpsi);
+ INT32_ANGLE_NORMALIZE(e->psi);
+
+}
+
+#endif /* AHRS_INT_UTILS_H */
diff --git a/sw/airborne/subsystems/electrical.c b/sw/airborne/subsystems/electrical.c
index e80426f697..125ccf5699 100644
--- a/sw/airborne/subsystems/electrical.c
+++ b/sw/airborne/subsystems/electrical.c
@@ -13,6 +13,9 @@ static struct {
#ifdef ADC_CHANNEL_CURRENT
struct adc_buf current_adc_buf;
#endif
+#ifdef MILLIAMP_AT_FULL_THROTTLE
+ float nonlin_factor;
+#endif
} electrical_priv;
#ifndef VoltageOfAdc
@@ -22,6 +25,9 @@ static struct {
#define MilliAmpereOfAdc(adc) DefaultMilliAmpereOfAdc(adc)
#endif
+#ifndef CURRENT_ESTIMATION_NONLINEARITY
+#define CURRENT_ESTIMATION_NONLINEARITY 1.2
+#endif
void electrical_init(void) {
electrical.vsupply = 0;
@@ -31,6 +37,10 @@ void electrical_init(void) {
#ifdef ADC_CHANNEL_CURRENT
adc_buf_channel(ADC_CHANNEL_CURRENT, &electrical_priv.current_adc_buf, DEFAULT_AV_NB_SAMPLE);
#endif
+
+#ifdef MILLIAMP_AT_FULL_THROTTLE
+ electrical_priv.nonlin_factor = CURRENT_ESTIMATION_NONLINEARITY;
+#endif
}
void electrical_periodic(void) {
@@ -44,7 +54,22 @@ void electrical_periodic(void) {
#endif
#else
#if defined MILLIAMP_AT_FULL_THROTTLE && defined COMMAND_THROTTLE
- electrical.current = ((float)commands[COMMAND_THROTTLE]) * ((float)MILLIAMP_AT_FULL_THROTTLE) / ((float)MAX_PPRZ);
+ /*
+ * Superellipse: abs(x/a)^n + abs(y/b)^n = 1
+ * with a = 1
+ * b = mA at full throttle
+ * n = 1.2 This defines nonlinearity (1 = linear)
+ * x = throttle
+ * y = current
+ *
+ * define CURRENT_ESTIMATION_NONLINEARITY in your airframe file to change the default nonlinearity factor of 1.2
+ */
+ float b = (float)MILLIAMP_AT_FULL_THROTTLE;
+ float x = ((float)commands[COMMAND_THROTTLE]) / ((float)MAX_PPRZ);
+ /* electrical.current y = ( b^n - (b* x/a)^n )^1/n
+ * a=1, n = electrical_priv.nonlin_factor
+ */
+ electrical.current = b - pow((pow(b,electrical_priv.nonlin_factor)-pow((b*x),electrical_priv.nonlin_factor)), (1./electrical_priv.nonlin_factor));
#endif
#endif /* ADC_CHANNEL_CURRENT */
diff --git a/sw/airborne/subsystems/imu.c b/sw/airborne/subsystems/imu.c
index e6baede3e8..515adc6b38 100644
--- a/sw/airborne/subsystems/imu.c
+++ b/sw/airborne/subsystems/imu.c
@@ -43,7 +43,7 @@ void imu_init(void) {
ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_THETA),
ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PSI) };
INT32_QUAT_OF_EULERS(imu.body_to_imu_quat, body_to_imu_eulers);
- INT32_QUAT_NORMALISE(imu.body_to_imu_quat);
+ INT32_QUAT_NORMALIZE(imu.body_to_imu_quat);
INT32_RMAT_OF_EULERS(imu.body_to_imu_rmat, body_to_imu_eulers);
#else
INT32_QUAT_ZERO(imu.body_to_imu_quat);
@@ -64,7 +64,7 @@ void imu_float_init(struct ImuFloat* imuf) {
EULERS_ASSIGN(imuf->body_to_imu_eulers,
IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI);
FLOAT_QUAT_OF_EULERS(imuf->body_to_imu_quat, imuf->body_to_imu_eulers);
- FLOAT_QUAT_NORMALISE(imuf->body_to_imu_quat);
+ FLOAT_QUAT_NORMALIZE(imuf->body_to_imu_quat);
FLOAT_RMAT_OF_EULERS(imuf->body_to_imu_rmat, imuf->body_to_imu_eulers);
#else
EULERS_ASSIGN(imuf->body_to_imu_eulers, 0., 0., 0.);
diff --git a/sw/airborne/subsystems/imu.h b/sw/airborne/subsystems/imu.h
index 57d1c4135c..43bf62a56f 100644
--- a/sw/airborne/subsystems/imu.h
+++ b/sw/airborne/subsystems/imu.h
@@ -61,14 +61,13 @@ struct ImuFloat {
};
extern void imu_float_init(struct ImuFloat* imuf);
+extern struct Imu imu;
/* underlying hardware */
#ifdef IMU_TYPE_H
#include IMU_TYPE_H
#endif
-extern struct Imu imu;
-
extern void imu_init(void);
#define ImuScaleGyro(_imu) { \
diff --git a/sw/airborne/subsystems/imu/imu_aspirin.c b/sw/airborne/subsystems/imu/imu_aspirin.c
index adb77c13a5..c8922066d2 100644
--- a/sw/airborne/subsystems/imu/imu_aspirin.c
+++ b/sw/airborne/subsystems/imu/imu_aspirin.c
@@ -32,6 +32,12 @@ void imu_impl_init(void) {
imu_aspirin.mag_available = FALSE;
imu_aspirin.accel_available = FALSE;
+ imu_aspirin.i2c_trans_gyro.type = I2CTransTxRx;
+ imu_aspirin.i2c_trans_gyro.buf[0] = ITG3200_REG_GYRO_XOUT_H;
+ imu_aspirin.i2c_trans_gyro.slave_addr = ITG3200_ADDR;
+ imu_aspirin.i2c_trans_gyro.len_w = 1;
+ imu_aspirin.i2c_trans_gyro.len_r = 6;
+
imu_aspirin_arch_init();
hmc5843_init();
@@ -43,10 +49,13 @@ void imu_periodic(void) {
if (imu_aspirin.status == AspirinStatusUninit) {
configure_gyro();
configure_accel();
+ imu_aspirin_arch_int_enable();
imu_aspirin.status = AspirinStatusIdle;
}
- else
+ else {
imu_aspirin.gyro_available_blaaa = TRUE;
+ imu_aspirin.time_since_last_reading++;
+ }
}
diff --git a/sw/airborne/subsystems/imu/imu_aspirin.h b/sw/airborne/subsystems/imu/imu_aspirin.h
index a76b8659c3..f937fb8fd5 100644
--- a/sw/airborne/subsystems/imu/imu_aspirin.h
+++ b/sw/airborne/subsystems/imu/imu_aspirin.h
@@ -71,9 +71,11 @@ struct ImuAspirin {
volatile uint8_t accel_available;
volatile uint8_t accel_tx_buf[7];
volatile uint8_t accel_rx_buf[7];
+ uint32_t time_since_last_reading;
};
extern struct ImuAspirin imu_aspirin;
+#define ASPIRIN_GYRO_TIMEOUT 3
#include "peripherals/hmc5843.h"
#define foo_handler() {}
@@ -88,43 +90,72 @@ extern struct ImuAspirin imu_aspirin;
} \
}
-#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \
- ImuMagEvent(_mag_handler); \
- if (imu_aspirin.status == AspirinStatusReadingGyro && \
- imu_aspirin.i2c_trans_gyro.status == I2CTransSuccess && i2c_idle(&i2c2)) { \
- int16_t gp = imu_aspirin.i2c_trans_gyro.buf[0]<<8 | imu_aspirin.i2c_trans_gyro.buf[1]; \
- int16_t gq = imu_aspirin.i2c_trans_gyro.buf[2]<<8 | imu_aspirin.i2c_trans_gyro.buf[3]; \
- int16_t gr = imu_aspirin.i2c_trans_gyro.buf[4]<<8 | imu_aspirin.i2c_trans_gyro.buf[5]; \
- RATES_ASSIGN(imu.gyro_unscaled, gp, gq, gr); \
- imu_aspirin.status = AspirinStatusIdle; \
- } \
- if (imu_aspirin.gyro_eoc && i2c2.status == I2CIdle && i2c_idle(&i2c2)) { \
- imu_aspirin.i2c_trans_gyro.type = I2CTransTxRx; \
- imu_aspirin.i2c_trans_gyro.buf[0] = ITG3200_REG_GYRO_XOUT_H; \
- imu_aspirin.i2c_trans_gyro.slave_addr = ITG3200_ADDR; \
- imu_aspirin.i2c_trans_gyro.len_w = 1; \
- imu_aspirin.i2c_trans_gyro.len_r = 6; \
- i2c_submit(&i2c2,&imu_aspirin.i2c_trans_gyro); \
- imu_aspirin.gyro_eoc = FALSE; \
- } \
- if (imu_aspirin.gyro_available_blaaa) { \
- imu_aspirin.gyro_available_blaaa = FALSE; \
- _gyro_handler(); \
- } \
- if (imu_aspirin.accel_available) { \
- imu_aspirin.accel_available = FALSE; \
- const int16_t ax = imu_aspirin.accel_rx_buf[1] | (imu_aspirin.accel_rx_buf[2]<<8); \
- const int16_t ay = imu_aspirin.accel_rx_buf[3] | (imu_aspirin.accel_rx_buf[4]<<8); \
- const int16_t az = imu_aspirin.accel_rx_buf[5] | (imu_aspirin.accel_rx_buf[6]<<8); \
- VECT3_ASSIGN(imu.accel_unscaled, ax, ay, az); \
- _accel_handler(); \
- } \
- }
-
-
/* underlying architecture */
#include "subsystems/imu/imu_aspirin_arch.h"
/* must be implemented by underlying architecture */
extern void imu_b2_arch_init(void);
+static inline void gyro_read_i2c(void)
+{
+ imu_aspirin.i2c_trans_gyro.buf[0] = ITG3200_REG_GYRO_XOUT_H;
+ i2c_submit(&i2c2,&imu_aspirin.i2c_trans_gyro);
+}
+
+static inline void gyro_copy_i2c(void)
+{
+ int16_t gp = imu_aspirin.i2c_trans_gyro.buf[0]<<8 | imu_aspirin.i2c_trans_gyro.buf[1];
+ int16_t gq = imu_aspirin.i2c_trans_gyro.buf[2]<<8 | imu_aspirin.i2c_trans_gyro.buf[3];
+ int16_t gr = imu_aspirin.i2c_trans_gyro.buf[4]<<8 | imu_aspirin.i2c_trans_gyro.buf[5];
+ RATES_ASSIGN(imu.gyro_unscaled, gp, gq, gr);
+}
+
+static inline void accel_copy_spi(void)
+{
+ const int16_t ax = imu_aspirin.accel_rx_buf[1] | (imu_aspirin.accel_rx_buf[2]<<8);
+ const int16_t ay = imu_aspirin.accel_rx_buf[3] | (imu_aspirin.accel_rx_buf[4]<<8);
+ const int16_t az = imu_aspirin.accel_rx_buf[5] | (imu_aspirin.accel_rx_buf[6]<<8);
+ VECT3_ASSIGN(imu.accel_unscaled, ax, ay, az);
+}
+
+static inline void imu_aspirin_event(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void))
+{
+ if (imu_aspirin.status == AspirinStatusUninit) return;
+
+ imu_aspirin_arch_int_disable();
+ ImuMagEvent(_mag_handler);
+ if (imu_aspirin.time_since_last_reading > ASPIRIN_GYRO_TIMEOUT) {
+ imu_aspirin.gyro_eoc = FALSE;
+ imu_aspirin.status = AspirinStatusIdle;
+ i2c2_er_irq_handler();
+ gyro_read_i2c();
+ imu_aspirin.time_since_last_reading = 0;
+ }
+ if (imu_aspirin.status == AspirinStatusReadingGyro && imu_aspirin.gyro_eoc &&
+ imu_aspirin.i2c_trans_gyro.status == I2CTransSuccess && i2c_idle(&i2c2)) {
+ gyro_copy_i2c();
+ imu_aspirin.status = AspirinStatusIdle;
+ if (imu_aspirin.gyro_available_blaaa) {
+ imu_aspirin.gyro_available_blaaa = FALSE;
+ _gyro_handler();
+ }
+ }
+ if (imu_aspirin.gyro_eoc && i2c2.status == I2CIdle && i2c_idle(&i2c2)) {
+ if (imu_aspirin.i2c_trans_gyro.status == I2CTransSuccess) {
+ imu_aspirin.time_since_last_reading = 0;
+ }
+ imu_aspirin.gyro_eoc = FALSE;
+ gyro_read_i2c();
+ }
+ if (imu_aspirin.accel_available) {
+ imu_aspirin.accel_available = FALSE;
+ accel_copy_spi();
+ _accel_handler();
+ }
+ imu_aspirin_arch_int_enable();
+}
+
+#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) do { \
+ imu_aspirin_event(_gyro_handler, _accel_handler, _mag_handler); \
+ } while(0);
+
#endif /* IMU_ASPIRIN_H */
diff --git a/sw/airborne/subsystems/imu/imu_b2.c b/sw/airborne/subsystems/imu/imu_b2.c
index 394bcc16da..5d4bf680a6 100644
--- a/sw/airborne/subsystems/imu/imu_b2.c
+++ b/sw/airborne/subsystems/imu/imu_b2.c
@@ -28,8 +28,8 @@ void imu_impl_init(void) {
imu_b2_arch_init();
max1168_init();
-#if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2001
- ms2001_init();
+#if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2100
+ ms2100_init();
#elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_AMI601
ami601_init();
#elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_HMC5843
diff --git a/sw/airborne/subsystems/imu/imu_b2.h b/sw/airborne/subsystems/imu/imu_b2.h
index bf9fd84ddf..91f7b34759 100644
--- a/sw/airborne/subsystems/imu/imu_b2.h
+++ b/sw/airborne/subsystems/imu/imu_b2.h
@@ -31,7 +31,7 @@
/* type of magnetometer */
#define IMU_B2_MAG_NONE 0
-#define IMU_B2_MAG_MS2001 1
+#define IMU_B2_MAG_MS2100 1
#define IMU_B2_MAG_AMI601 2
@@ -142,14 +142,14 @@
#endif /* IMU_B2_VERSION_1_2 */
-#if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2001
-#include "peripherals/ms2001.h"
+#if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2100
+#include "peripherals/ms2100.h"
#define ImuMagEvent(_mag_handler) { \
- if (ms2001_status == MS2001_DATA_AVAILABLE) { \
- imu.mag_unscaled.x = ms2001_values[IMU_MAG_X_CHAN]; \
- imu.mag_unscaled.y = ms2001_values[IMU_MAG_Y_CHAN]; \
- imu.mag_unscaled.z = ms2001_values[IMU_MAG_Z_CHAN]; \
- ms2001_status = MS2001_IDLE; \
+ if (ms2100_status == MS2100_DATA_AVAILABLE) { \
+ imu.mag_unscaled.x = ms2100_values[IMU_MAG_X_CHAN]; \
+ imu.mag_unscaled.y = ms2100_values[IMU_MAG_Y_CHAN]; \
+ imu.mag_unscaled.z = ms2100_values[IMU_MAG_Z_CHAN]; \
+ ms2100_status = MS2100_IDLE; \
_mag_handler(); \
} \
}
diff --git a/sw/airborne/subsystems/imu/imu_dummy.c b/sw/airborne/subsystems/imu/imu_dummy.c
new file mode 100644
index 0000000000..8ef21d644c
--- /dev/null
+++ b/sw/airborne/subsystems/imu/imu_dummy.c
@@ -0,0 +1,7 @@
+
+
+void imu_impl_init(void) {
+
+
+}
+
diff --git a/sw/airborne/subsystems/ins.c b/sw/airborne/subsystems/ins.c
index cd098c51b5..0e8e560273 100644
--- a/sw/airborne/subsystems/ins.c
+++ b/sw/airborne/subsystems/ins.c
@@ -70,7 +70,7 @@ bool_t ins_hf_realign;
int32_t ins_qfe;
bool_t ins_baro_initialised;
int32_t ins_baro_alt;
-#ifdef BOOZ2_SONAR
+#ifdef USE_SONAR
bool_t ins_update_on_agl;
int32_t ins_sonar_offset;
#endif
@@ -190,7 +190,7 @@ void ins_update_baro() {
if (ins_vf_realign) {
ins_vf_realign = FALSE;
ins_qfe = baro.absolute;
-#ifdef BOOZ2_SONAR
+#ifdef USE_SONAR
ins_sonar_offset = sonar_meas;
#endif
vff_realign(0.);
@@ -267,13 +267,13 @@ void ins_update_gps(void) {
}
void ins_update_sonar() {
-#if defined BOOZ2_SONAR && defined USE_VFF
+#if defined USE_SONAR && defined USE_VFF
static int32_t sonar_filtered = 0;
sonar_filtered = (sonar_meas + 2*sonar_filtered) / 3;
/* update baro_qfe assuming a flat ground */
- if (ins_update_on_agl && booz2_analog_baro_status == BOOZ2_ANALOG_BARO_RUNNING) {
+ if (ins_update_on_agl && baro.status == BS_RUNNING) {
int32_t d_sonar = (((int32_t)sonar_filtered - ins_sonar_offset) * INS_SONAR_SENS_NUM) / INS_SONAR_SENS_DEN;
- ins_qfe = (int32_t)booz2_analog_baro_value + (d_sonar * (INS_BARO_SENS_DEN))/INS_BARO_SENS_NUM;
+ ins_qfe = baro.absolute + (d_sonar * (INS_BARO_SENS_DEN))/INS_BARO_SENS_NUM;
}
#endif
}
diff --git a/sw/airborne/subsystems/ins.h b/sw/airborne/subsystems/ins.h
index d2d374c121..6353bbb553 100644
--- a/sw/airborne/subsystems/ins.h
+++ b/sw/airborne/subsystems/ins.h
@@ -37,7 +37,7 @@ extern struct NedCoor_i ins_gps_speed_cm_s_ned;
extern int32_t ins_baro_alt;
extern int32_t ins_qfe;
extern bool_t ins_baro_initialised;
-#ifdef BOOZ2_SONAR
+#ifdef USE_SONAR
extern bool_t ins_update_on_agl; /* use sonar to update agl if available */
extern int32_t ins_sonar_offset;
#endif
diff --git a/sw/airborne/subsystems/navigation/poly_survey_adv.c b/sw/airborne/subsystems/navigation/poly_survey_adv.c
new file mode 100644
index 0000000000..b56ff6f579
--- /dev/null
+++ b/sw/airborne/subsystems/navigation/poly_survey_adv.c
@@ -0,0 +1,332 @@
+#include "poly_survey_adv.h"
+
+#include "subsystems/nav.h"
+#include "estimator.h"
+#include "autopilot.h"
+#include "generated/flight_plan.h"
+//#include "modules/digital_cam/dc.h"
+
+
+/**
+The following variables are set by poly_survey_init and not changed later on
+**/
+
+// precomputed vectors to ease calculations
+point2d dir_vec;
+point2d sweep_vec;
+point2d rad_vec;
+
+//the polygon from the flightplan
+uint8_t poly_first;
+uint8_t poly_count;
+
+//desired properties of the flyover
+float psa_min_rad;
+float psa_sweep_width;
+float psa_shot_dist;
+float psa_altitude;
+
+//direction for the flyover (0° == N)
+int segment_angle;
+int return_angle;
+
+/**
+The Following variables are dynamic, changed while navigating.
+**/
+
+/*
+psa_stage starts at ENTRY and than circles trought the other
+states until to polygon is completely covered
+ENTRY : getting in the right position and height for the first flyover
+SEG : fly from seg_start to seg_end and take pictures,
+ then calculate navigation points of next flyover
+TURN1 : do a 180° turn around seg_center1
+RET : fly from ret_start to ret_end
+TURN2 : do a 180° turn around seg_center2
+*/
+survey_stage psa_stage;
+
+// points for navigation
+point2d seg_start;
+point2d seg_end;
+point2d seg_center1;
+point2d seg_center2;
+point2d entry_center;
+point2d ret_start;
+point2d ret_end;
+
+
+//helper functions and macro
+#define VEC_CALC(A, B, C, OP) A.x = B.x OP C.x; A.y = B.y OP C.y;
+
+static point2d vec_add(point2d a, point2d b)
+{
+ point2d tmp;
+ VEC_CALC(tmp, a, b, +);
+
+ return tmp;
+}
+
+static void nav_points(point2d start, point2d end)
+{
+ nav_route_xy(start.x, start.y, end.x, end.y);
+}
+
+/**
+ intercept two lines and give back the point of intersection
+ returns : FALSE if no intersection can be found or intersection does not lie between points a and b
+ else TRUE
+ p : returns intersection
+ x, y : first line is defined by point x and y (goes through this points)
+ a1, a2, b1, b2 : second line by coordinates a1/a2, b1/b2
+**/
+static bool_t intercept_two_lines(point2d *p, point2d x, point2d y, float a1, float a2, float b1, float b2)
+{
+ float div, fac;
+
+ div = (((b2 - a2)*(y.x - x.x)) + ((x.y - y.y)*(b1 - a1)));
+ if (div == 0) return FALSE;
+ fac = ((y.x*(x.y - a2)) + (x.x*(a2 - y.y)) + (a1*(y.y - x.y))) / div;
+ if (fac > 1.0) return FALSE;
+ if (fac < 0.0) return FALSE;
+
+ p->x = a1 + fac*(b1 - a1);
+ p->y = a2 + fac*(b2 - a2);
+
+ return TRUE;
+}
+
+/**
+ intersects a line with the polygon and gives back the two intersection points
+ returns : TRUE if two intersection can be found, else FALSE
+ x, y : intersection points
+ a, b : define the line to intersection
+**/
+static bool_t get_two_intersects(point2d *x, point2d *y, point2d a, point2d b)
+{
+ int i, count = 0;
+ point2d tmp;
+
+ for (i=0;i fabs(dir_vec.y)) {
+ if ((y->x - x->x) / dir_vec.x < 0.0){
+ tmp = *x;
+ *x = *y;
+ *y = tmp;
+ }
+ }
+ else
+ if ((y->y - x->y) / dir_vec.y < 0.0) {
+ tmp = *x;
+ *x = *y;
+ *y = tmp;
+ }
+
+ return TRUE;
+}
+
+/**
+ initializes the variables needed for the survey to start
+ first_wp : the first Waypoint of the polygon
+ size : the number of points that make up the polygon
+ angle : angle in which to do the flyovers
+ sweep_width : distance between the sweeps
+ shot_dist : distance between the shots
+ min_rad : minimal radius when navigating
+ altitude : the altitude that must be reached before the flyover starts
+**/
+bool_t init_poly_survey_adv(uint8_t first_wp, uint8_t size, float angle, float sweep_width, float shot_dist, float min_rad, float altitude)
+{
+ int i;
+ point2d small, sweep;
+ float div, len, angle_rad = angle/180.0*M_PI;
+
+ if (angle < 0.0) angle += 360.0;
+ if (angle >= 360.0) angle -= 360.0;
+
+ poly_first = first_wp;
+ poly_count = size;
+
+ psa_sweep_width = sweep_width;
+ psa_min_rad = min_rad;
+ psa_shot_dist = shot_dist;
+ psa_altitude = altitude;
+
+ segment_angle = angle;
+ return_angle = angle+180;
+ if (return_angle > 359) return_angle -= 360;
+
+ if (angle <= 45.0 || angle >= 315.0) {
+ //north
+ dir_vec.y = 1.0;
+ dir_vec.x = 1.0*tanf(angle_rad);
+ sweep.x = 1.0;
+ sweep.y = - dir_vec.x / dir_vec.y;
+ }
+ else if (angle <= 135.0) {
+ //east
+ dir_vec.x = 1.0;
+ dir_vec.y = 1.0/tanf(angle_rad);
+ sweep.y = - 1.0;
+ sweep.x = dir_vec.y / dir_vec.x;
+ }
+ else if (angle <= 225.0) {
+ //south
+ dir_vec.y = -1.0;
+ dir_vec.x = -1.0*tanf(angle_rad);
+ sweep.x = -1.0;
+ sweep.y = dir_vec.x / dir_vec.y;
+ }
+ else {
+ //west
+ dir_vec.x = -1.0;
+ dir_vec.y = -1.0/tanf(angle_rad);
+ sweep.y = 1.0;
+ sweep.x = - dir_vec.y / dir_vec.x;
+ }
+
+ //normalize
+ len = sqrt(sweep.x*sweep.x+sweep.y*sweep.y);
+ sweep.x = sweep.x / len;
+ sweep.y = sweep.y / len;
+
+ rad_vec.x = sweep.x * psa_min_rad;
+ rad_vec.y = sweep.y * psa_min_rad;
+ sweep_vec.x = sweep.x * psa_sweep_width;
+ sweep_vec.y = sweep.y * psa_sweep_width;
+
+ //begin at leftmost position (relative to dir_vec)
+ small.x = waypoints[poly_first].x;
+ small.y = waypoints[poly_first].y;
+
+ div = (sweep_vec.y*dir_vec.x) - (sweep_vec.x*dir_vec.y);
+
+ //cacluate the leftmost point if one sees the dir vec as going "up" and the sweep vec as going right
+ if (div < 0.0) {
+ for(i=1;i 0.0) {
+ small.x = waypoints[poly_first+i].x;
+ small.y = waypoints[poly_first+i].y;
+ }
+ }
+ else
+ for(i=1;i 0.0) {
+ small.x = waypoints[poly_first+i].x;
+ small.y = waypoints[poly_first+i].y;
+ }
+
+ //calculate the line the defines the first flyover
+ seg_start.x = small.x + 0.5*sweep_vec.x;
+ seg_start.y = small.y + 0.5*sweep_vec.y;
+ VEC_CALC(seg_end, seg_start, dir_vec, +);
+
+ if (!get_two_intersects(&seg_start, &seg_end, seg_start, seg_end)) {
+ psa_stage = ERR;
+ return FALSE;
+ }
+
+ //center of the entry circle
+ entry_center.x = seg_start.x - rad_vec.x;
+ entry_center.y = seg_start.y - rad_vec.y;
+
+ //fast climbing to desired altitude
+ NavVerticalAutoThrottleMode(100.0);
+ NavVerticalAltitudeMode(psa_altitude, 0.0);
+
+ psa_stage = ENTRY;
+
+ return FALSE;
+}
+
+/**
+ main navigation routine. This is called periodically evaluates the current
+ Position and stage and navigates accordingly.
+ Returns True until the survey is finished
+**/
+bool_t poly_survey_adv(void)
+{
+ //entry circle around entry-center until the desired altitude is reached
+ if (psa_stage == ENTRY) {
+ nav_circle_XY(entry_center.x, entry_center.y, -psa_min_rad);
+ if (NavCourseCloseTo(segment_angle)
+ && nav_approaching_xy(seg_start.x, seg_start.y, last_x, last_y, CARROT)
+ && fabs(estimator_z - psa_altitude) <= 20) {
+ psa_stage = SEG;
+ NavVerticalAutoThrottleMode(0.0);
+ nav_init_stage();
+ //dc_distance(psa_shot_dist, seg_start.x - dir_vec.x*psa_shot_dist*0.5, seg_start.y - dir_vec.y*psa_shot_dist*0.5);
+ }
+ }
+ //fly the segment until seg_end is reached
+ if (psa_stage == SEG) {
+ nav_points(seg_start, seg_end);
+ //calculate all needed points for the next flyover
+ if (nav_approaching_xy(seg_end.x, seg_end.y, seg_start.x, seg_start.y, 0)) {
+ //dc_stop();
+ VEC_CALC(seg_center1, seg_end, rad_vec, -);
+ ret_start.x = seg_end.x - 2*rad_vec.x;
+ ret_start.y = seg_end.y - 2*rad_vec.y;
+
+ //if we get no intersection the survey is finished
+ if (!get_two_intersects(&seg_start, &seg_end, vec_add(seg_start, sweep_vec), vec_add(seg_end, sweep_vec)))
+ return FALSE;
+
+ ret_end.x = seg_start.x - sweep_vec.x - 2*rad_vec.x;
+ ret_end.y = seg_start.y - sweep_vec.y - 2*rad_vec.y;
+
+ seg_center2.x = seg_start.x - 0.5*(2.0*rad_vec.x+sweep_vec.x);
+ seg_center2.y = seg_start.y - 0.5*(2.0*rad_vec.y+sweep_vec.y);
+
+ psa_stage = TURN1;
+ nav_init_stage();
+ }
+ }
+ //turn from stage to return
+ else if (psa_stage == TURN1) {
+ nav_circle_XY(seg_center1.x, seg_center1.y, -psa_min_rad);
+ if (NavCourseCloseTo(return_angle)) {
+ psa_stage = RET;
+ nav_init_stage();
+ }
+ //return
+ } else if (psa_stage == RET) {
+ nav_points(ret_start, ret_end);
+ if (nav_approaching_xy(ret_end.x, ret_end.y, ret_start.x, ret_start.y, 0)) {
+ psa_stage = TURN2;
+ nav_init_stage();
+ }
+ //turn from return to stage
+ } else if (psa_stage == TURN2) {
+ nav_circle_XY(seg_center2.x, seg_center2.y, -(2*psa_min_rad+psa_sweep_width)*0.5);
+ if (NavCourseCloseTo(segment_angle)) {
+ psa_stage = SEG;
+ nav_init_stage();
+ //dc_distance(psa_shot_dist, seg_start.x - dir_vec.x*psa_shot_dist*0.5, seg_start.y - dir_vec.y*psa_shot_dist*0.5);
+ }
+ }
+
+ return TRUE;
+}
diff --git a/sw/airborne/subsystems/navigation/poly_survey_adv.h b/sw/airborne/subsystems/navigation/poly_survey_adv.h
new file mode 100644
index 0000000000..90b0164565
--- /dev/null
+++ b/sw/airborne/subsystems/navigation/poly_survey_adv.h
@@ -0,0 +1,13 @@
+#ifndef POLY_ADV_H
+#define POLY_ADV_H
+
+#include "std.h"
+
+typedef struct {float x; float y;} point2d;
+
+typedef enum {ERR, ENTRY, SEG, TURN1, RET, TURN2} survey_stage;
+
+extern bool_t init_poly_survey_adv(uint8_t first_wp, uint8_t size, float angle, float sweep_width, float shot_dist, float min_rad, float altitude);
+extern bool_t poly_survey_adv(void);
+
+#endif
diff --git a/sw/airborne/subsystems/navigation/spiral.c b/sw/airborne/subsystems/navigation/spiral.c
new file mode 100644
index 0000000000..9ab41fb2b1
--- /dev/null
+++ b/sw/airborne/subsystems/navigation/spiral.c
@@ -0,0 +1,161 @@
+/************** Spiral Navigation **********************************************/
+
+/** creating a helix:
+ start radius to end radius, increasing after reaching alphamax
+ Alphamax is calculated from given segments
+ IMPORTANT: numer of segments has to be larger than 2!
+*/
+
+#include "spiral.h"
+
+#include "subsystems/nav.h"
+#include "estimator.h"
+#include "autopilot.h"
+#include "generated/flight_plan.h"
+//#include "modules/digital_cam/dc.h"
+
+enum SpiralStatus { Outside, StartCircle, Circle, IncSpiral };
+static enum SpiralStatus CSpiralStatus;
+// static float SpiralTheta;
+// static float Fly2X;
+// static float Fly2Y;
+
+static float FlyFromX;
+static float FlyFromY;
+static float TransCurrentX;
+static float TransCurrentY;
+static float TransCurrentZ;
+static float EdgeCurrentX;
+static float EdgeCurrentY;
+static float LastCircleX;
+static float LastCircleY;
+static float DistanceFromCenter;
+static float Spiralradius;
+static uint8_t Center;
+static uint8_t Edge;
+static float SRad;
+static float IRad;
+static float Alphalimit;
+static float Segmente;
+static float CamAngle;
+static float ZPoint;
+static float nav_radius_min;
+
+#ifndef MIN_CIRCLE_RADIUS
+#define MIN_CIRCLE_RADIUS 120
+#endif
+
+
+bool_t InitializeSpiral(uint8_t CenterWP, uint8_t EdgeWP, float StartRad, float IncRad, float Segments, float ZKoord)
+{
+ Center = CenterWP; // center of the helix
+ Edge = EdgeWP; // edge point on the maximaum radius
+ SRad = StartRad; // start radius of the helix
+ Segmente = Segments;
+ ZPoint = ZKoord;
+ nav_radius_min = MIN_CIRCLE_RADIUS;
+ if (SRad < nav_radius_min) SRad = nav_radius_min;
+ IRad = IncRad; // multiplier for increasing the spiral
+
+ EdgeCurrentX = waypoints[Edge].x - waypoints[Center].x;
+ EdgeCurrentY = waypoints[Edge].y - waypoints[Center].y;
+
+ Spiralradius = sqrt(EdgeCurrentX*EdgeCurrentX+EdgeCurrentY*EdgeCurrentY);
+
+ TransCurrentX = estimator_x - waypoints[Center].x;
+ TransCurrentY = estimator_y - waypoints[Center].y;
+ TransCurrentZ = estimator_z - ZPoint;
+ DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY);
+
+ // SpiralTheta = atan2(TransCurrentY,TransCurrentX);
+ // Fly2X = Spiralradius*cos(SpiralTheta+3.14)+waypoints[Center].x;
+ // Fly2Y = Spiralradius*sin(SpiralTheta+3.14)+waypoints[Center].y;
+
+ // Alphalimit denotes angle, where the radius will be increased
+ Alphalimit = 2*3.14 / Segments;
+ //current position
+ FlyFromX = estimator_x;
+ FlyFromY = estimator_y;
+
+ if(DistanceFromCenter > Spiralradius)
+ CSpiralStatus = Outside;
+ return FALSE;
+}
+
+bool_t SpiralNav(void)
+{
+ TransCurrentX = estimator_x - waypoints[Center].x;
+ TransCurrentY = estimator_y - waypoints[Center].y;
+ DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY);
+
+ bool_t InCircle = TRUE;
+
+ if(DistanceFromCenter > Spiralradius)
+ InCircle = FALSE;
+
+ switch(CSpiralStatus)
+ {
+ case Outside:
+ //flys until center of the helix is reached an start helix
+ nav_route_xy(FlyFromX,FlyFromY,waypoints[Center].x, waypoints[Center].y);
+ // center reached?
+ if (nav_approaching_xy(waypoints[Center].x, waypoints[Center].y, FlyFromX, FlyFromY, 0)) {
+ // nadir image
+ //dc_shutter();
+ CSpiralStatus = StartCircle;
+ }
+ break;
+ case StartCircle:
+ // Starts helix
+ // storage of current coordinates
+ // calculation needed, State switch to Circle
+ nav_circle_XY(waypoints[Center].x, waypoints[Center].y, SRad);
+ if(DistanceFromCenter >= SRad){
+ LastCircleX = estimator_x;
+ LastCircleY = estimator_y;
+ CSpiralStatus = Circle;
+ // Start helix
+ //dc_Circle(360/Segmente);
+ }
+ break;
+ case Circle: {
+ nav_circle_XY(waypoints[Center].x, waypoints[Center].y, SRad);
+ // Trigonometrische Berechnung des bereits geflogenen Winkels alpha
+ // equation:
+ // alpha = 2 * asin ( |Starting position angular - current positon| / (2* SRad)
+ // if alphamax already reached, increase radius.
+
+ //DistanceStartEstim = |Starting position angular - current positon|
+ float DistanceStartEstim = sqrt (((LastCircleX-estimator_x)*(LastCircleX-estimator_x))
+ + ((LastCircleY-estimator_y)*(LastCircleY-estimator_y)));
+ float CircleAlpha = (2.0 * asin (DistanceStartEstim / (2 * SRad)));
+ if (CircleAlpha >= Alphalimit) {
+ LastCircleX = estimator_x;
+ LastCircleY = estimator_y;
+ CSpiralStatus = IncSpiral;
+ }
+ break;
+ }
+ case IncSpiral:
+ // increasing circle radius as long as it is smaller than max helix radius
+ if(SRad + IRad < Spiralradius)
+ {
+ SRad = SRad + IRad;
+ /*if (dc_cam_tracing) {
+ // calculating Camwinkel for camera alignment
+ TransCurrentZ = estimator_z - ZPoint;
+ CamAngle = atan(SRad/TransCurrentZ) * 180 / 3.14;
+ //dc_cam_angle = CamAngle;
+ }*/
+ }
+ else {
+ SRad = Spiralradius;
+ // Stopps DC
+ //dc_stop();
+ }
+ CSpiralStatus = Circle;
+ break;
+
+ }
+ return TRUE;
+}
diff --git a/sw/airborne/subsystems/navigation/spiral.h b/sw/airborne/subsystems/navigation/spiral.h
new file mode 100644
index 0000000000..7eea57cd54
--- /dev/null
+++ b/sw/airborne/subsystems/navigation/spiral.h
@@ -0,0 +1,11 @@
+#ifndef SPIRAL_H
+#define SPIRAL_H
+
+#include "std.h"
+
+extern bool_t SpiralNav(void);
+extern bool_t InitializeSpiral(uint8_t CenterWP, uint8_t EdgeWP, float StartRad, float IncRad,
+ float Segments, float ZKoord );
+
+#endif
+
diff --git a/sw/airborne/subsystems/radio_control/rc_datalink.c b/sw/airborne/subsystems/radio_control/rc_datalink.c
index 662a6b5ccd..1d7a39ccb5 100644
--- a/sw/airborne/subsystems/radio_control/rc_datalink.c
+++ b/sw/airborne/subsystems/radio_control/rc_datalink.c
@@ -21,7 +21,7 @@
* Boston, MA 02111-1307, USA.
*/
-#include "radio_control/rc_datalink.h"
+#include "subsystems/radio_control/rc_datalink.h"
#include "subsystems/radio_control.h"
int8_t rc_dl_values[ RC_DL_NB_CHANNEL ];
@@ -33,11 +33,11 @@ void radio_control_impl_init(void) {
}
-void parse_rc_datalink( uint8_t throttle_mode,
+void parse_rc_3ch_datalink( uint8_t throttle_mode,
int8_t roll,
int8_t pitch)
{
- uint8_t throttle = throttle_mode & 0xFC;
+ uint8_t throttle = ((throttle_mode & 0xFC)>>2)*(128/64);
uint8_t mode = throttle_mode & 0x03;
rc_dl_values[RADIO_ROLL] = roll;
@@ -47,3 +47,20 @@ void parse_rc_datalink( uint8_t throttle_mode,
rc_dl_frame_available = TRUE;
}
+
+void parse_rc_4ch_datalink(
+ uint8_t mode,
+ uint8_t throttle,
+ int8_t roll,
+ int8_t pitch,
+ int8_t yaw)
+{
+ rc_dl_values[RADIO_MODE] = (int8_t)mode;
+ rc_dl_values[RADIO_THROTTLE] = (int8_t)throttle;
+ rc_dl_values[RADIO_ROLL] = roll;
+ rc_dl_values[RADIO_PITCH] = pitch;
+ rc_dl_values[RADIO_YAW] = yaw;
+
+ rc_dl_frame_available = TRUE;
+}
+
diff --git a/sw/airborne/subsystems/radio_control/rc_datalink.h b/sw/airborne/subsystems/radio_control/rc_datalink.h
index d10170febb..ca72750480 100644
--- a/sw/airborne/subsystems/radio_control/rc_datalink.h
+++ b/sw/airborne/subsystems/radio_control/rc_datalink.h
@@ -43,13 +43,24 @@ extern int8_t rc_dl_values[ RC_DL_NB_CHANNEL ];
extern volatile bool_t rc_dl_frame_available;
/**
- * Decode datalink message to get rc values
+ * Decode datalink message to get rc values with RC_3CH message
+ * Mode and throttle are merge in the same byte
*/
-extern void parse_rc_datalink(
+extern void parse_rc_3ch_datalink(
uint8_t throttle_mode,
int8_t roll,
int8_t pitch);
+/**
+ * Decode datalink message to get rc values with RC_4CH message
+ */
+extern void parse_rc_4ch_datalink(
+ uint8_t mode,
+ uint8_t throttle,
+ int8_t roll,
+ int8_t pitch,
+ int8_t yaw);
+
/**
* Macro that normalize rc_dl_values to radio values
*/
@@ -60,7 +71,7 @@ extern void parse_rc_datalink(
Bound(_out[RADIO_PITCH], MIN_PPRZ, MAX_PPRZ); \
_out[RADIO_YAW] = 0; \
Bound(_out[RADIO_YAW], MIN_PPRZ, MAX_PPRZ); \
- _out[RADIO_THROTTLE] = ((MAX_PPRZ/64) * _in[RADIO_THROTTLE]); \
+ _out[RADIO_THROTTLE] = ((MAX_PPRZ/128) * _in[RADIO_THROTTLE]); \
Bound(_out[RADIO_THROTTLE], 0, MAX_PPRZ); \
_out[RADIO_MODE] = MAX_PPRZ * (_in[RADIO_MODE] - 1); \
Bound(_out[RADIO_MODE], MIN_PPRZ, MAX_PPRZ); \
diff --git a/sw/airborne/subsystems/settings.c b/sw/airborne/subsystems/settings.c
new file mode 100644
index 0000000000..04754e2ead
--- /dev/null
+++ b/sw/airborne/subsystems/settings.c
@@ -0,0 +1,21 @@
+#include "subsystems/settings.h"
+
+
+struct PersistentSettings pers_settings;
+bool_t settings_store_now;
+
+
+void settings_init(void) {
+#ifdef USE_PERSISTENT_SETTINGS
+ if (persistent_read((uint32_t)&pers_settings, sizeof(struct PersistentSettings)))
+ return; // return -1 ?
+ persitent_settings_load();
+#endif
+}
+
+
+void settings_store(void) {
+ persitent_settings_store();
+ persistent_write((uint32_t)&pers_settings, sizeof(struct PersistentSettings));
+}
+
diff --git a/sw/airborne/subsystems/settings.h b/sw/airborne/subsystems/settings.h
new file mode 100644
index 0000000000..8c39b680eb
--- /dev/null
+++ b/sw/airborne/subsystems/settings.h
@@ -0,0 +1,20 @@
+#ifndef SUBSYSTEMS_SETTINGS_H
+#define SUBSYSTEMS_SETTINGS_H
+
+#include "std.h"
+
+extern void settings_init(void);
+extern void settings_store(void);
+
+extern bool_t settings_store_now;
+
+#define settings_StoreSettings(_v) { settings_store_now = _v; settings_store(); }
+
+#include "generated/settings.h"
+
+/* implemented in arch dependant code */
+int32_t persistent_write(uint32_t ptr, uint32_t size);
+int32_t persistent_read(uint32_t ptr, uint32_t size);
+
+
+#endif /* SUBSYSTEMS_SETTINGS_H */
diff --git a/sw/airborne/test/ahrs/Makefile b/sw/airborne/test/ahrs/Makefile
index 196de6300d..2cde23ddab 100644
--- a/sw/airborne/test/ahrs/Makefile
+++ b/sw/airborne/test/ahrs/Makefile
@@ -1,21 +1,31 @@
+# Launch with "make Q=''" to get full command display
+Q=@
CC = gcc
CFLAGS = -std=c99 -I.. -I../.. -I../../../include -I../../booz -I../../../booz -Wall
-LDFLAGS = -lm
+LDFLAGS = -lm -lgsl -lgslcblas
+
+CFLAGS +=
# imu wants airframe to fetch its neutrals
# ahrs wants airframe to fetch IMU_BODY_TO_IMU_ANGLES
-CFLAGS += -I../../../../var/BOOZ2_A7
+#CFLAGS += -I../../../../var/BOOZ2_A7
+CFLAGS += -I../../../../var/TA
#CFLAGS += -DIMU_BODY_TO_IMU_PHI=0 -DIMU_BODY_TO_IMU_THETA=0 -DIMU_BODY_TO_IMU_PSI=0
# toulouse 0.51562740288882, -0.05707735220832, 0.85490967783446
CFLAGS += -DAHRS_H_X=0.51562740288882 -DAHRS_H_Y=-0.05707735220832 -DAHRS_H_Z=0.85490967783446
-#CFLAGS += -DOUTPUT_IN_BODY_FRAME
-CFLAGS += -DENABLE_MAG_UPDATE
-CFLAGS += -DENABLE_ACCEL_UPDATE
+#CFLAGS += -DDISABLE_ALIGNEMENT
+#CFLAGS += -DDISABLE_PROPAGATE
+#CFLAGS += -DDISABLE_ACCEL_UPDATE
+#CFLAGS += -DDISABLE_MAG_UPDATE
+#CFLAGS += -DPERFECT_SENSORS
+#CFLAGS += -DLOW_NOISE_THRESHOLD='ACCEL_BFP_OF_REAL(5.5)'
+CFLAGS += -DLOW_NOISE_THRESHOLD=500000
+CFLAGS += -DLOW_NOISE_TIME=2
SRCS= run_ahrs_on_flight_log.c \
../../math/pprz_trig_int.c \
@@ -34,5 +44,102 @@ run_ahrs_fcr_on_flight_log: ../../subsystems/ahrs/ahrs_float_cmpl_rmat.c $(SRCS)
run_ahrs_ice_on_flight_log: ../../subsystems/ahrs/ahrs_int_cmpl_euler.c $(SRCS)
$(CC) -DAHRS_TYPE=AHRS_TYPE_ICE $(CFLAGS) -o $@ $^ $(LDFLAGS)
+ifndef AHRS_TYPE
+#AHRS_TYPE = AHRS_TYPE_ICE
+AHRS_TYPE = AHRS_TYPE_ICQ
+#AHRS_TYPE = AHRS_TYPE_FLQ
+#AHRS_TYPE = AHRS_TYPE_FCR # doesn't work - inverted accel
+#AHRS_TYPE = AHRS_TYPE_FCR2
+#AHRS_TYPE = AHRS_TYPE_FCQ
+endif
+ifndef PROPAGATE_LOW_PASS_RATES
+PROPAGATE_LOW_PASS_RATES = 0
+endif
+ifndef GRAVITY_UPDATE_NORM_HEURISTIC
+GRAVITY_UPDATE_NORM_HEURISTIC = 0
+endif
+ifndef GRAVITY_UPDATE_COORDINATED_TURN
+GRAVITY_UPDATE_COORDINATED_TURN = 0
+endif
+ifndef MAG_UPDATE_YAW_ONLY
+MAG_UPDATE_YAW_ONLY = 0
+endif
+
+
+AHRS_CFLAGS += -DAHRS_TYPE=$(AHRS_TYPE)
+AHRS_CFLAGS += -DPERIODIC_FREQUENCY=512
+AHRS_CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=512
+
+ifeq ($(PROPAGATE_LOW_PASS_RATES), 1)
+AHRS_CFLAGS += -DAHRS_PROPAGATE_LOW_PASS_RATES
+endif
+ifeq ($(GRAVITY_UPDATE_NORM_HEURISTIC), 1)
+AHRS_CFLAGS += -DAHRS_GRAVITY_UPDATE_NORM_HEURISTIC
+endif
+ifeq ($(GRAVITY_UPDATE_COORDINATED_TURN), 1)
+AHRS_CFLAGS += -DAHRS_GRAVITY_UPDATE_COORDINATED_TURN
+endif
+ifeq ($(MAG_UPDATE_YAW_ONLY), 1)
+AHRS_CFLAGS += -DAHRS_MAG_UPDATE_YAW_ONLY
+endif
+
+AHRS_SRCS += ../../subsystems/ahrs.c \
+ ../../subsystems/ahrs/ahrs_aligner.c \
+
+ifeq ($(AHRS_TYPE), AHRS_TYPE_ICE)
+AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_euler.h\"
+AHRS_CFLAGS += -DFACE_REINJ_1=1024
+AHRS_SRCS += ../../subsystems/ahrs/ahrs_int_cmpl_euler.c
+endif
+ifeq ($(AHRS_TYPE), AHRS_TYPE_ICQ)
+AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl.h\"
+AHRS_SRCS += ../../subsystems/ahrs/ahrs_int_cmpl.c
+endif
+ifeq ($(AHRS_TYPE), AHRS_TYPE_FLQ)
+AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_lkf_quat.h\"
+AHRS_SRCS += ../../subsystems/ahrs/ahrs_float_lkf_quat.c
+endif
+ifeq ($(AHRS_TYPE), AHRS_TYPE_FCR)
+AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm.h\"
+AHRS_CFLAGS += -DINS_ROLL_NEUTRAL_DEFAULT=0
+AHRS_CFLAGS += -DINS_PITCH_NEUTRAL_DEFAULT=0
+AHRS_CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=512
+AHRS_CFLAGS += -DDCM_UPDATE_AFTER_PROPAGATE
+AHRS_SRCS += ../../subsystems/ahrs/ahrs_float_dcm.c
+endif
+ifeq ($(AHRS_TYPE), AHRS_TYPE_FCR2)
+AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_rmat.h\"
+AHRS_CFLAGS += -DAHRS_PROPAGATE_RMAT
+AHRS_SRCS += ../../subsystems/ahrs/ahrs_float_cmpl_rmat.c
+endif
+ifeq ($(AHRS_TYPE), AHRS_TYPE_FCQ)
+AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_rmat.h\"
+AHRS_CFLAGS += -DAHRS_PROPAGATE_QUAT
+AHRS_SRCS += ../../subsystems/ahrs/ahrs_float_cmpl_rmat.c
+endif
+
+
+RAOS_SRCS = ./ahrs_on_synth.c \
+ ../../subsystems/imu.c \
+ ../../subsystems/imu/imu_dummy.c \
+ ../../math/pprz_trig_int.c \
+ ../../../simulator/nps/nps_random.c
+
+
+run_ahrs_on_synth: run_ahrs_on_synth.c $(RAOS_SRCS) $(AHRS_SRCS)
+ @echo "Building run_ahrs_on_synth for $(AHRS_TYPE)"
+ $(Q) $(CC) $(CFLAGS) $(AHRS_CFLAGS) -o $@ $^ $(LDFLAGS)
+
+IVY_CFLAGS=-g -O2 -Wall `pkg-config glib-2.0 --cflags`
+IVY_LDFLAGS=`pkg-config glib-2.0 --libs` `pcre-config --libs` -lglibivy
+
+run_ahrs_on_synth_ivy: run_ahrs_on_synth_ivy.c $(RAOS_SRCS) $(AHRS_SRCS)
+ $(CC) $(CFLAGS) $(AHRS_CFLAGS) $(IVY_CFLAGS) -o $@ $^ $(LDFLAGS) $(IVY_LDFLAGS)
+
+
+
+
+
clean:
- rm -f *~ run_ahrs_*_on_flight_log
+ @echo "cleaning ..."
+ $(Q) rm -f *~ run_ahrs_*_on_flight_log run_ahrs_on_synth_ivy run_ahrs_on_synth
diff --git a/sw/airborne/test/ahrs/ahrs_on_synth.c b/sw/airborne/test/ahrs/ahrs_on_synth.c
new file mode 100644
index 0000000000..21d451a307
--- /dev/null
+++ b/sw/airborne/test/ahrs/ahrs_on_synth.c
@@ -0,0 +1,413 @@
+#include "test/ahrs/ahrs_on_synth.h"
+
+#include
+
+#include "subsystems/imu.h"
+#include "subsystems/ahrs.h"
+#include "subsystems/ahrs/ahrs_aligner.h"
+#include "../simulator/nps/nps_random.h"
+
+#include "../pprz_algebra_print.h"
+
+
+char* ahrs_type_str[AHRS_TYPE_NB] = {
+ "Int Compl Euler",
+ "Int Compl Quat",
+ "Float LKF Quat",
+ "Float Compl Rmat",
+ "Float Compl Rmat 2",
+ "Float Compl Quat" };
+
+static void traj_static_static_init(void);
+static void traj_static_static_update(void);
+
+static void traj_step_phi_init(void);
+static void traj_step_phi_update(void);
+
+static void traj_step_phi_2nd_order_init(void);
+static void traj_step_phi_2nd_order_update(void);
+
+static void traj_step_biasp_init(void);
+static void traj_step_biasp_update(void);
+
+static void traj_static_sine_init(void);
+static void traj_static_sine_update(void);
+
+static void traj_sineX_quad_init(void);
+static void traj_sineX_quad_update(void);
+
+static void traj_coordinated_circle_init(void);
+static void traj_coordinated_circle_update(void);
+
+static void traj_stop_stop_x_init(void);
+static void traj_stop_stop_x_update(void);
+
+struct traj traj[] = {
+ {.name="static", .desc="blaa",
+ .init_fun=traj_static_static_init, .update_fun=traj_static_static_update},
+ {.name="sine", .desc="blaa2",
+ .init_fun=traj_static_sine_init, .update_fun=traj_static_sine_update},
+ {.name="sineX", .desc="blaa2",
+ .init_fun=traj_sineX_quad_init, .update_fun=traj_sineX_quad_update},
+ {.name="step_phi", .desc="blaa2",
+ .init_fun=traj_step_phi_init, .update_fun=traj_step_phi_update},
+ {.name="step_phi2", .desc="blaa2",
+ .init_fun=traj_step_phi_2nd_order_init, .update_fun=traj_step_phi_2nd_order_update},
+ {.name="step_bias", .desc="blaa2",
+ .init_fun=traj_step_biasp_init, .update_fun=traj_step_biasp_update},
+ {.name="coordinated circle", .desc="blaa2",
+ .init_fun=traj_coordinated_circle_init, .update_fun=traj_coordinated_circle_update},
+ {.name="stop stop x", .desc="blaa2",
+ .init_fun=traj_stop_stop_x_init, .update_fun=traj_stop_stop_x_update}
+};
+
+
+struct AhrsOnSynth aos;
+
+
+void aos_init(int traj_nb) {
+
+ aos.traj = &traj[traj_nb];
+
+ aos.time = 0;
+ aos.dt = 1./AHRS_PROPAGATE_FREQUENCY;
+ aos.traj->ts = 0;
+ aos.traj->ts = 1.; // default to one second
+
+ /* default state */
+ EULERS_ASSIGN(aos.ltp_to_imu_euler, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.));
+ FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler);
+ RATES_ASSIGN(aos.imu_rates, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.));
+ FLOAT_VECT3_ZERO(aos.ltp_pos);
+ FLOAT_VECT3_ZERO(aos.ltp_vel);
+ FLOAT_VECT3_ZERO(aos.ltp_accel);
+ FLOAT_VECT3_ZERO(aos.ltp_jerk);
+ aos.traj->init_fun();
+
+ imu_init();
+ ahrs_init();
+ ahrs_aligner_init();
+
+#ifdef PERFECT_SENSORS
+ RATES_ASSIGN(aos.gyro_bias, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.));
+ RATES_ASSIGN(aos.gyro_noise, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.));
+ VECT3_ASSIGN(aos.accel_noise, 0., 0., 0.);
+#else
+ RATES_ASSIGN(aos.gyro_bias, RadOfDeg(1.), RadOfDeg(2.), RadOfDeg(3.));
+ RATES_ASSIGN(aos.gyro_noise, RadOfDeg(1.), RadOfDeg(1.), RadOfDeg(1.));
+ VECT3_ASSIGN(aos.accel_noise, .5, .5, .5);
+#endif
+
+
+#ifdef FORCE_ALIGNEMENT
+ // DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("# oas quat", aos.ltp_to_imu_quat);
+ aos_compute_sensors();
+ // DISPLAY_FLOAT_RATES_DEG("# oas gyro_bias", aos.gyro_bias);
+ // DISPLAY_FLOAT_RATES_DEG("# oas imu_rates", aos.imu_rates);
+ VECT3_COPY(ahrs_aligner.lp_accel, imu.accel);
+ VECT3_COPY(ahrs_aligner.lp_mag, imu.mag);
+ RATES_COPY(ahrs_aligner.lp_gyro, imu.gyro);
+ // DISPLAY_INT32_RATES_AS_FLOAT_DEG("# ahrs_aligner.lp_gyro", ahrs_aligner.lp_gyro);
+ ahrs_align();
+ // DISPLAY_FLOAT_RATES_DEG("# ahrs_impl.gyro_bias", ahrs_impl.gyro_bias);
+
+#endif
+
+
+
+
+#ifdef DISABLE_ALIGNEMENT
+ printf("# DISABLE_ALIGNEMENT\n");
+#endif
+#ifdef DISABLE_PROPAGATE
+ printf("# DISABLE_PROPAGATE\n");
+#endif
+#ifdef DISABLE_ACCEL_UPDATE
+ printf("# DISABLE_ACCEL_UPDATE\n");
+#endif
+#ifdef DISABLE_MAG_UPDATE
+ printf("# DISABLE_MAG_UPDATE\n");
+#endif
+ printf("# AHRS_TYPE %s\n", ahrs_type_str[AHRS_TYPE]);
+ printf("# AHRS_PROPAGATE_FREQUENCY %d\n", AHRS_PROPAGATE_FREQUENCY);
+#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
+ printf("# AHRS_PROPAGATE_LOW_PASS_RATES\n");
+#endif
+#ifdef AHRS_MAG_UPDATE_YAW_ONLY
+ printf("# AHRS_MAG_UPDATE_YAW_ONLY\n");
+#endif
+#ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN
+ printf("# AHRS_GRAVITY_UPDATE_COORDINATED_TURN\n");
+#endif
+#ifdef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
+ printf("# AHRS_GRAVITY_UPDATE_NORM_HEURISTIC\n");
+#endif
+#ifdef PERFECT_SENSORS
+ printf("# PERFECT_SENSORS\n");
+#endif
+
+ printf("# tajectory : %s\n", aos.traj->name);
+
+};
+
+
+void aos_compute_sensors(void) {
+
+ struct FloatRates gyro;
+ RATES_SUM(gyro, aos.imu_rates, aos.gyro_bias);
+ // printf("#aos.gyro_bias %f\n",DegOfRad( aos.gyro_bias.r));
+
+ float_rates_add_gaussian_noise(&gyro, &aos.gyro_noise);
+
+ RATES_BFP_OF_REAL(imu.gyro, gyro);
+ RATES_BFP_OF_REAL(imu.gyro_prev, gyro);
+
+ struct FloatVect3 g_ltp = {0., 0., 9.81};
+ struct FloatVect3 accelero_ltp;
+ VECT3_DIFF(accelero_ltp, aos.ltp_accel, g_ltp);
+ struct FloatVect3 accelero_imu;
+ FLOAT_QUAT_VMULT(accelero_imu, aos.ltp_to_imu_quat, accelero_ltp);
+
+ float_vect3_add_gaussian_noise(&accelero_imu, &aos.accel_noise);
+ ACCELS_BFP_OF_REAL(imu.accel, accelero_imu);
+
+
+ struct FloatVect3 h_earth = {AHRS_H_X, AHRS_H_Y, AHRS_H_Z};
+ struct FloatVect3 h_imu;
+ FLOAT_QUAT_VMULT(h_imu, aos.ltp_to_imu_quat, h_earth);
+ MAGS_BFP_OF_REAL(imu.mag, h_imu);
+
+#ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN
+#if AHRS_TYPE == AHRS_TYPE_FCR2 || AHRS_TYPE == AHRS_TYPE_FCQ || AHRS_TYPE == AHRS_TYPE_FLQ
+ VECT3_COPY(ahrs_impl.est_ltp_speed, aos.ltp_vel);
+#endif
+#if AHRS_TYPE == AHRS_TYPE_ICQ
+ ahrs_impl.ltp_vel_norm = SPEED_BFP_OF_REAL(FLOAT_VECT3_NORM(aos.ltp_vel));
+#endif
+#endif
+
+
+}
+
+void aos_compute_state(void) {
+
+ aos.time += aos.dt;
+ aos.traj->update_fun();
+
+}
+
+
+void aos_run(void) {
+
+ aos_compute_state();
+ aos_compute_sensors();
+#ifndef DISABLE_ALIGNEMENT
+ if (ahrs.status == AHRS_UNINIT) {
+ ahrs_aligner_run();
+ if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED)
+ ahrs_align();
+ }
+ else {
+#endif /* DISABLE_ALIGNEMENT */
+ ahrs_propagate();
+ ahrs_update_accel();
+ ahrs_update_mag();
+#ifndef DISABLE_ALIGNEMENT
+ }
+#endif
+
+}
+
+
+
+
+static void traj_static_static_init(void) {
+
+ aos.traj->te = 120.;
+
+}
+
+static void traj_static_static_update(void) {
+
+ // if (aos.time > 3) {
+ // EULERS_ASSIGN(aos.ltp_to_imu_euler, RadOfDeg(5), 0, 0);
+ // FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler);
+ // }
+ // aos.imu_rates.p = 0.;
+ // aos.imu_rates.q = 0.;
+ // aos.imu_rates.r = 0.;
+
+}
+
+
+//
+//
+//
+static void traj_static_sine_init(void) {
+
+ aos.traj->te = 10.;
+
+}
+
+static void traj_static_sine_update(void) {
+
+
+ aos.imu_rates.p = RadOfDeg(200)*cos(aos.time);
+ aos.imu_rates.q = RadOfDeg(200)*cos(0.7*aos.time+2);
+ aos.imu_rates.r = RadOfDeg(200)*cos(0.8*aos.time+1);
+ FLOAT_QUAT_INTEGRATE(aos.ltp_to_imu_quat, aos.imu_rates, aos.dt);
+ FLOAT_EULERS_OF_QUAT(aos.ltp_to_imu_euler, aos.ltp_to_imu_quat);
+
+}
+
+
+//
+// this is a sine trajectory along the x axis
+// we pretend a dragless vectoriel trust vehicle banks
+// to achieve it
+//
+static void traj_sineX_quad_init(void) { aos.traj->te = 60.; }
+static void traj_sineX_quad_update(void) {
+
+ const float om = RadOfDeg(10);
+
+ if ( aos.time > (M_PI/om) ) {
+ const float a = 20;
+
+ struct FloatVect3 jerk;
+ VECT3_ASSIGN(jerk , -a*om*om*om*cos(om*aos.time), 0, 0);
+ VECT3_ASSIGN(aos.ltp_accel , -a*om*om *sin(om*aos.time), 0, 0);
+ VECT3_ASSIGN(aos.ltp_vel , a*om *cos(om*aos.time), 0, 0);
+ VECT3_ASSIGN(aos.ltp_pos , a *sin(om*aos.time), 0, 0);
+
+ // this is based on differential flatness of the quad
+ EULERS_ASSIGN(aos.ltp_to_imu_euler, 0., atan2(aos.ltp_accel.x, 9.81), 0.);
+ FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler);
+ const struct FloatEulers e_dot = {
+ 0.,
+ 9.81*jerk.x / ( (9.81*9.81) + (aos.ltp_accel.x*aos.ltp_accel.x)),
+ 0. };
+ FLOAT_RATES_OF_EULER_DOT(aos.imu_rates, aos.ltp_to_imu_euler, e_dot);
+ }
+}
+
+
+//
+//
+//
+static void traj_step_phi_init(void) { aos.traj->te = 40.;}
+static void traj_step_phi_update(void) {
+ if (aos.time > 5) {
+ EULERS_ASSIGN(aos.ltp_to_imu_euler, RadOfDeg(5), 0, 0);
+ FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler);
+ }
+}
+
+//
+//
+//
+static void traj_step_phi_2nd_order_init(void) {
+ aos.traj->te = 0.;
+ aos.traj->te = 40.;
+}
+
+static void traj_step_phi_2nd_order_update(void) {
+
+ if (aos.time > 15) {
+ const float omega = RadOfDeg(100);
+ const float xi = 0.9;
+ struct FloatRates raccel;
+ RATES_ASSIGN(raccel, -2.*xi*omega*aos.imu_rates.p - omega*omega*(aos.ltp_to_imu_euler.phi - RadOfDeg(5)), 0., 0.);
+ FLOAT_RATES_INTEGRATE_FI(aos.imu_rates, raccel, aos.dt);
+ FLOAT_QUAT_INTEGRATE(aos.ltp_to_imu_quat, aos.imu_rates, aos.dt);
+ FLOAT_EULERS_OF_QUAT(aos.ltp_to_imu_euler, aos.ltp_to_imu_quat);
+ }
+
+}
+
+
+static void traj_step_biasp_init(void) { aos.traj->te = 120.; }
+static void traj_step_biasp_update(void) { if (aos.time > 5) aos.gyro_bias.p = RadOfDeg(3);}
+
+
+static void traj_coordinated_circle_init(void) {
+
+ aos.traj->te = 120.;
+
+ const float speed = 15; // m/s
+ const float R = 100; // radius in m
+ // tan phi = v^2/Rg
+ float phi = atan2(speed*speed, R*9.81);
+ EULERS_ASSIGN(aos.ltp_to_imu_euler, phi, 0, M_PI_2);
+ FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler);
+}
+
+static void traj_coordinated_circle_update(void) {
+ const float speed = 15; // m/s
+ const float R = 100; // radius in m
+ float omega = speed / R;
+ // tan phi = v^2/Rg
+ float phi = atan2(speed*speed, R*9.81);
+ if ( aos.time > 2.*M_PI/omega) {
+ VECT3_ASSIGN(aos.ltp_pos, R*cos(omega*aos.time), R*sin(omega*aos.time), 0.);
+ VECT3_ASSIGN(aos.ltp_vel, -omega*R*sin(omega*aos.time), omega*R*cos(omega*aos.time), 0.);
+ VECT3_ASSIGN(aos.ltp_accel, -omega*omega*R*cos(omega*aos.time), -omega*omega*R*sin(omega*aos.time), 0.);
+
+
+ // float psi = atan2(aos.ltp_pos.y, aos.ltp_pos.x);
+ float psi = M_PI_2 + omega*aos.time; while (psi>M_PI) psi -= 2.*M_PI;
+ EULERS_ASSIGN(aos.ltp_to_imu_euler, phi, 0, psi);
+ FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler);
+
+ struct FloatEulers e_dot;
+ EULERS_ASSIGN(e_dot, 0., 0., omega);
+ FLOAT_RATES_OF_EULER_DOT(aos.imu_rates, aos.ltp_to_imu_euler, e_dot);
+ }
+
+}
+
+
+
+//static char** traj_stop_stop_x_desc(void) {
+ // static const char** desc =
+ // {"stop top", NULL};
+// return desc;
+//}
+static void traj_stop_stop_x_init(void) { aos.traj->te = 30.;}
+
+static void traj_stop_stop_x_update(void){
+
+ const float t0 = 5.;
+ const float dt_jerk = 0.75;
+ const float dt_nojerk = 10.;
+ const float val_jerk = 5.;
+
+ FLOAT_VECT3_INTEGRATE_FI(aos.ltp_pos, aos.ltp_vel, aos.dt);
+ FLOAT_VECT3_INTEGRATE_FI(aos.ltp_vel, aos.ltp_accel, aos.dt);
+ FLOAT_VECT3_INTEGRATE_FI(aos.ltp_accel, aos.ltp_jerk, aos.dt);
+
+ if (aos.time < t0) return;
+ else if (aos.time < t0+dt_jerk) {
+ VECT3_ASSIGN(aos.ltp_jerk , val_jerk, 0., 0.); }
+ else if (aos.time < t0 + 2.*dt_jerk) {
+ VECT3_ASSIGN(aos.ltp_jerk , -val_jerk, 0., 0.); }
+ else if (aos.time < t0 + 2.*dt_jerk + dt_nojerk) {
+ VECT3_ASSIGN(aos.ltp_jerk , 0. , 0., 0.); }
+ else if (aos.time < t0 + 3.*dt_jerk + dt_nojerk) {
+ VECT3_ASSIGN(aos.ltp_jerk , -val_jerk, 0., 0.); }
+ else if (aos.time < t0 + 4.*dt_jerk + dt_nojerk) {
+ VECT3_ASSIGN(aos.ltp_jerk , val_jerk, 0., 0.); }
+ else {
+ VECT3_ASSIGN(aos.ltp_jerk , 0. , 0., 0.); }
+
+
+ // this is based on differential flatness of the quad
+ EULERS_ASSIGN(aos.ltp_to_imu_euler, 0., atan2(aos.ltp_accel.x, 9.81), 0.);
+ FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler);
+ const struct FloatEulers e_dot = {
+ 0.,
+ 9.81*aos.ltp_jerk.x / ( (9.81*9.81) + (aos.ltp_accel.x*aos.ltp_accel.x)),
+ 0. };
+ FLOAT_RATES_OF_EULER_DOT(aos.imu_rates, aos.ltp_to_imu_euler, e_dot);
+
+}
diff --git a/sw/airborne/test/ahrs/ahrs_on_synth.h b/sw/airborne/test/ahrs/ahrs_on_synth.h
new file mode 100644
index 0000000000..20644a59db
--- /dev/null
+++ b/sw/airborne/test/ahrs/ahrs_on_synth.h
@@ -0,0 +1,60 @@
+#ifndef AHRS_ON_SYNTH_H
+#define AHRS_ON_SYNTH_H
+
+#include "math/pprz_algebra_float.h"
+
+struct traj {
+ char* name;
+ char* desc;
+ void (*init_fun)(void);
+ void (*update_fun)(void);
+
+ double ts;
+ double te;
+};
+
+struct AhrsOnSynth {
+
+ struct traj* traj;
+
+ double time;
+ double dt;
+
+ /* sensors */
+ struct FloatRates gyro_bias;
+ struct FloatRates gyro_noise;
+ struct FloatVect3 accel_noise;
+
+ /* state */
+ struct FloatEulers ltp_to_imu_euler;
+ struct FloatQuat ltp_to_imu_quat;
+ struct FloatRates imu_rates;
+
+ struct FloatVect3 ltp_jerk;
+ struct FloatVect3 ltp_accel;
+ struct FloatVect3 ltp_vel;
+ struct FloatVect3 ltp_pos;
+
+
+
+};
+
+extern struct AhrsOnSynth aos;
+
+extern void aos_init(int traj_nb);
+extern void aos_run(void);
+extern void aos_compute_sensors(void);
+extern void aos_compute_state(void);
+
+
+#define AHRS_TYPE_ICE 0
+#define AHRS_TYPE_ICQ 1
+#define AHRS_TYPE_FLQ 2
+#define AHRS_TYPE_FCR 3
+#define AHRS_TYPE_FCR2 4
+#define AHRS_TYPE_FCQ 5
+#define AHRS_TYPE_NB 6
+
+extern char* ahrs_type_str[AHRS_TYPE_NB];
+
+#endif /* AHRS_ON_SYNTH_H */
diff --git a/sw/airborne/test/ahrs/ahrs_utils.py b/sw/airborne/test/ahrs/ahrs_utils.py
new file mode 100644
index 0000000000..a31d502d5a
--- /dev/null
+++ b/sw/airborne/test/ahrs/ahrs_utils.py
@@ -0,0 +1,158 @@
+#! /usr/bin/env python
+
+# $Id$
+# Copyright (C) 2011 Antoine Drouin
+#
+# This file is part of Paparazzi.
+#
+# Paparazzi is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2, or (at your option)
+# any later version.
+#
+# Paparazzi is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with Paparazzi; see the file COPYING. If not, write to
+# the Free Software Foundation, 59 Temple Place - Suite 330,
+# Boston, MA 02111-1307, USA.
+#
+
+#import os
+#from optparse import OptionParser
+#import scipy
+#from scipy import optimize
+import shlex, subprocess
+from pylab import *
+from array import array
+import numpy
+import matplotlib.pyplot as plt
+
+def run_simulation(ahrs_type, build_opt, traj_nb):
+ print "\nBuilding ahrs"
+ args = ["make", "clean", "run_ahrs_on_synth", "AHRS_TYPE=AHRS_TYPE_"+ahrs_type] + build_opt
+# print args
+ p = subprocess.Popen(args=args, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, shell=False)
+ outputlines = p.stdout.readlines()
+ p.wait()
+ for i in outputlines:
+ print " # "+i,
+ print
+
+ print "Running simulation"
+ print " using traj " + str(traj_nb)
+ p = subprocess.Popen(args=["./run_ahrs_on_synth",str(traj_nb)], stdout=subprocess.PIPE, stderr=subprocess.STDOUT, shell=False)
+ outputlines = p.stdout.readlines()
+ p.wait()
+# for i in outputlines:
+# print " "+i,
+# print "\n"
+
+ ahrs_data_type = [('time', 'float32'),
+ ('phi_true', 'float32'), ('theta_true', 'float32'), ('psi_true', 'float32'),
+ ('p_true', 'float32'), ('q_true', 'float32'), ('r_true', 'float32'),
+ ('bp_true', 'float32'), ('bq_true', 'float32'), ('br_true', 'float32'),
+ ('phi_ahrs', 'float32'), ('theta_ahrs', 'float32'), ('psi_ahrs', 'float32'),
+ ('p_ahrs', 'float32'), ('q_ahrs', 'float32'), ('r_ahrs', 'float32'),
+ ('bp_ahrs', 'float32'), ('bq_ahrs', 'float32'), ('br_ahrs', 'float32')
+ ]
+ pos_data_type = [ ('x0_true', 'float32'), ('y0_true', 'float32'), ('z0_true', 'float32'),
+ ('x1_true', 'float32'), ('y1_true', 'float32'), ('z1_true', 'float32'),
+ ('x2_true', 'float32'), ('y2_true', 'float32'), ('z2_true', 'float32'),
+ ('x3_true', 'float32'), ('y3_true', 'float32'), ('z3_true', 'float32'),
+ ]
+ mydescr = numpy.dtype(ahrs_data_type)
+ data = [[] for dummy in xrange(len(mydescr))]
+# import code; code.interact(local=locals())
+ for line in outputlines:
+ if line.startswith("#"):
+ print " "+line,
+ else:
+ fields = line.strip().split(' ');
+# print fields
+ for i, number in enumerate(fields):
+ data[i].append(number)
+
+ print
+ for i in xrange(len(mydescr)):
+ data[i] = cast[mydescr[i]](data[i])
+
+ return numpy.rec.array(data, dtype=mydescr)
+
+
+
+def plot_simulation_results(plot_true_state, lsty, sim_res):
+ print "Plotting Results"
+
+ # f, (ax1, ax2, ax3) = plt.subplots(3, sharex=True, sharey=True)
+
+ subplot(3,3,1)
+ plt.plot(sim_res.time, sim_res.phi_ahrs, lsty)
+ ylabel('degres')
+ title('phi')
+
+ subplot(3,3,2)
+ plot(sim_res.time, sim_res.theta_ahrs, lsty)
+ title('theta')
+
+ subplot(3,3,3)
+ plot(sim_res.time, sim_res.psi_ahrs, lsty)
+ title('psi')
+
+ subplot(3,3,4)
+ plt.plot(sim_res.time, sim_res.p_ahrs, lsty)
+ ylabel('degres/s')
+ title('p')
+
+ subplot(3,3,5)
+ plt.plot(sim_res.time, sim_res.q_ahrs, lsty)
+ title('q')
+
+ subplot(3,3,6)
+ plt.plot(sim_res.time, sim_res.r_ahrs, lsty)
+ title('r')
+
+ subplot(3,3,7)
+ plt.plot(sim_res.time, sim_res.bp_ahrs, lsty)
+ ylabel('degres/s')
+ xlabel('time in s')
+ title('bp')
+
+ subplot(3,3,8)
+ plt.plot(sim_res.time, sim_res.bq_ahrs, lsty)
+ xlabel('time in s')
+ title('bq')
+
+ subplot(3,3,9)
+ plt.plot(sim_res.time, sim_res.br_ahrs, lsty)
+ xlabel('time in s')
+ title('br')
+
+
+ if plot_true_state:
+ subplot(3,3,1)
+ plt.plot(sim_res.time, sim_res.phi_true, 'r--')
+ subplot(3,3,2)
+ plot(sim_res.time, sim_res.theta_true, 'r--')
+ subplot(3,3,3)
+ plot(sim_res.time, sim_res.psi_true, 'r--')
+ subplot(3,3,4)
+ plot(sim_res.time, sim_res.p_true, 'r--')
+ subplot(3,3,5)
+ plot(sim_res.time, sim_res.q_true, 'r--')
+ subplot(3,3,6)
+ plot(sim_res.time, sim_res.r_true, 'r--')
+ subplot(3,3,7)
+ plot(sim_res.time, sim_res.bp_true, 'r--')
+ subplot(3,3,8)
+ plot(sim_res.time, sim_res.bq_true, 'r--')
+ subplot(3,3,9)
+ plot(sim_res.time, sim_res.br_true, 'r--')
+
+
+
+def show_plot():
+ plt.show();
diff --git a/sw/airborne/test/ahrs/compare_ahrs.py b/sw/airborne/test/ahrs/compare_ahrs.py
new file mode 100755
index 0000000000..8ac53e56ec
--- /dev/null
+++ b/sw/airborne/test/ahrs/compare_ahrs.py
@@ -0,0 +1,67 @@
+#! /usr/bin/env python
+
+# $Id$
+# Copyright (C) 2011 Antoine Drouin
+#
+# This file is part of Paparazzi.
+#
+# Paparazzi is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2, or (at your option)
+# any later version.
+#
+# Paparazzi is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with Paparazzi; see the file COPYING. If not, write to
+# the Free Software Foundation, 59 Temple Place - Suite 330,
+# Boston, MA 02111-1307, USA.
+#
+
+import ahrs_utils
+
+def main():
+
+# traj_nb = 0; # static
+# traj_nb = 1; # sine orientation
+# traj_nb = 2; # sine X quad
+ traj_nb = 3; # step_phi
+# traj_nb = 4; # step_phi_2nd_order
+# traj_nb = 5; # step_bias
+# traj_nb = 6; # coordinated circle
+# traj_nb = 7; # stop stop X quad
+
+ build_opt1 = [];
+# build_opt1 = ["Q="];
+ build_opt1 += ["PROPAGATE_LOW_PASS_RATES=1"];
+# build_opt1 = ["GRAVITY_UPDATE_COORDINATED_TURN=1"];
+ build_opt1 += ["GRAVITY_UPDATE_NORM_HEURISTIC=0"];
+# build_opt1 = ["MAG_UPDATE_YAW_ONLY=1"];
+ ahrs_type1 = "FCQ";
+# ahrs_type1 = "FCR2";
+# ahrs_type1 = "FLQ";
+# ahrs_type1 = "ICQ";
+ sim_res1 = ahrs_utils.run_simulation(ahrs_type1, build_opt1, traj_nb)
+# import code; code.interact(local=locals())
+
+ build_opt2 = [];
+# build_opt2 = ["GRAVITY_UPDATE_COORDINATED_TURN=1"];
+ build_opt2 += ["GRAVITY_UPDATE_NORM_HEURISTIC=1"];
+# build_opt2 = ["MAG_UPDATE_YAW_ONLY=0"];
+ build_opt2 += ["PROPAGATE_LOW_PASS_RATES=1"];
+# build_opt2 = build_opt1;
+# ahrs_type2 = "FLQ";
+ ahrs_type2 = "FCQ";
+# ahrs_type2 = "ICQ";
+ ahrs_type2 = ahrs_type1;
+ sim_res2 = ahrs_utils.run_simulation(ahrs_type2, build_opt2, traj_nb)
+
+ ahrs_utils.plot_simulation_results(False, 'b', sim_res1)
+ ahrs_utils.plot_simulation_results(True, 'g', sim_res2)
+ ahrs_utils.show_plot()
+
+if __name__ == "__main__":
+ main()
diff --git a/sw/airborne/test/ahrs/run_ahrs_on_synth.c b/sw/airborne/test/ahrs/run_ahrs_on_synth.c
new file mode 100644
index 0000000000..ec0e4dc6cb
--- /dev/null
+++ b/sw/airborne/test/ahrs/run_ahrs_on_synth.c
@@ -0,0 +1,92 @@
+#include
+
+#include "test/ahrs/ahrs_on_synth.h"
+
+#include "subsystems/imu.h"
+#include "subsystems/ahrs.h"
+#include "subsystems/ahrs/ahrs_aligner.h"
+
+#include "math/pprz_algebra_float.h"
+#include "math/pprz_algebra_int.h"
+#include "test/pprz_algebra_print.h"
+
+
+static void report(void);
+
+int main(int argc, char** argv) {
+
+ int traj = 0;
+ if (argc > 1)
+ traj = atoi(argv[1]);
+
+ aos_init(traj);
+ report();
+
+ // while (aos.time < 0.01) {
+ while (aos.time < aos.traj->te) {
+ aos_run();
+ report();
+ }
+
+ return 0;
+}
+
+
+static void report(void) {
+
+ int output_sensors = FALSE;
+ int output_pos = FALSE;
+
+#if AHRS_TYPE == AHRS_TYPE_ICE || AHRS_TYPE == AHRS_TYPE_ICQ
+ EULERS_FLOAT_OF_BFP(ahrs_float.ltp_to_imu_euler, ahrs.ltp_to_imu_euler);
+ RATES_FLOAT_OF_BFP(ahrs_float.imu_rate, ahrs.imu_rate);
+#endif
+ printf("%f ", aos.time);
+
+ printf("%f %f %f ", DegOfRad(aos.ltp_to_imu_euler.phi),
+ DegOfRad(aos.ltp_to_imu_euler.theta),
+ DegOfRad(aos.ltp_to_imu_euler.psi));
+
+ printf("%f %f %f ", DegOfRad(aos.imu_rates.p),
+ DegOfRad(aos.imu_rates.q),
+ DegOfRad(aos.imu_rates.r));
+
+ printf("%f %f %f ", DegOfRad(aos.gyro_bias.p),
+ DegOfRad(aos.gyro_bias.q),
+ DegOfRad(aos.gyro_bias.r));
+
+
+ printf("%f %f %f ", DegOfRad(ahrs_float.ltp_to_imu_euler.phi),
+ DegOfRad(ahrs_float.ltp_to_imu_euler.theta),
+ DegOfRad(ahrs_float.ltp_to_imu_euler.psi));
+
+ printf("%f %f %f ", DegOfRad(ahrs_float.imu_rate.p),
+ DegOfRad(ahrs_float.imu_rate.q),
+ DegOfRad(ahrs_float.imu_rate.r));
+
+#if AHRS_TYPE == AHRS_TYPE_ICQ
+ struct FloatRates bias;
+ RATES_FLOAT_OF_BFP(bias, ahrs_impl.gyro_bias);
+ printf("%f %f %f ", DegOfRad(bias.p),
+ DegOfRad(bias.q),
+ DegOfRad(bias.r));
+#endif
+#if AHRS_TYPE == AHRS_TYPE_FLQ || AHRS_TYPE == AHRS_TYPE_FCR2 || AHRS_TYPE == AHRS_TYPE_FCQ
+ printf("%f %f %f ", DegOfRad(ahrs_impl.gyro_bias.p),
+ DegOfRad(ahrs_impl.gyro_bias.q),
+ DegOfRad(ahrs_impl.gyro_bias.r));
+#endif
+
+ if (output_pos) {
+ printf("%f %f %f ", aos.ltp_pos.x,
+ aos.ltp_pos.y,
+ aos.ltp_pos.z);
+ }
+
+ if (output_sensors) {
+
+ }
+
+ printf("\n");
+
+}
diff --git a/sw/airborne/test/ahrs/run_ahrs_on_synth_ivy.c b/sw/airborne/test/ahrs/run_ahrs_on_synth_ivy.c
new file mode 100644
index 0000000000..f9809717f7
--- /dev/null
+++ b/sw/airborne/test/ahrs/run_ahrs_on_synth_ivy.c
@@ -0,0 +1,102 @@
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+
+#include "test/ahrs/ahrs_on_synth.h"
+
+#include "subsystems/imu.h"
+#include "subsystems/ahrs.h"
+
+#include "math/pprz_algebra_float.h"
+#include "math/pprz_algebra_int.h"
+#include "test/pprz_algebra_print.h"
+
+
+
+
+gboolean timeout_callback(gpointer data) {
+
+ for (int i=0; i<20; i++) {
+ aos_compute_state();
+ aos_compute_sensors();
+#ifndef DISABLE_PROPAGATE
+ ahrs_propagate();
+#endif
+#ifndef DISABLE_ACCEL_UPDATE
+ ahrs_update_accel();
+#endif
+#ifndef DISABLE_MAG_UPDATE
+ if (!(i%5)) ahrs_update_mag();
+#endif
+ }
+
+#if AHRS_TYPE == AHRS_TYPE_ICE || AHRS_TYPE == AHRS_TYPE_ICQ
+ EULERS_FLOAT_OF_BFP(ahrs_float.ltp_to_imu_euler, ahrs.ltp_to_imu_euler);
+#endif
+
+#if AHRS_TYPE == AHRS_TYPE_ICQ
+ IvySendMsg("183 BOOZ_AHRS_BIAS %d %d %d",
+ ahrs_impl.gyro_bias.p,
+ ahrs_impl.gyro_bias.q,
+ ahrs_impl.gyro_bias.r);
+#endif
+#if AHRS_TYPE == AHRS_TYPE_FLQ || AHRS_TYPE == AHRS_TYPE_FCR2
+ struct Int32Rates bias_i;
+ RATES_BFP_OF_REAL(bias_i, ahrs_impl.gyro_bias);
+ IvySendMsg("183 BOOZ_AHRS_BIAS %d %d %d",
+ bias_i.p,
+ bias_i.q,
+ bias_i.r);
+#endif
+
+ IvySendMsg("183 AHRS_EULER %f %f %f",
+ ahrs_float.ltp_to_imu_euler.phi,
+ ahrs_float.ltp_to_imu_euler.theta,
+ ahrs_float.ltp_to_imu_euler.psi);
+
+ IvySendMsg("183 BOOZ_SIM_RATE_ATTITUDE %f %f %f %f %f %f",
+ DegOfRad(aos.imu_rates.p),
+ DegOfRad(aos.imu_rates.q),
+ DegOfRad(aos.imu_rates.r),
+ DegOfRad(aos.ltp_to_imu_euler.phi),
+ DegOfRad(aos.ltp_to_imu_euler.theta),
+ DegOfRad(aos.ltp_to_imu_euler.psi));
+
+ IvySendMsg("183 BOOZ_SIM_GYRO_BIAS %f %f %f",
+ DegOfRad(aos.gyro_bias.p),
+ DegOfRad(aos.gyro_bias.q),
+ DegOfRad(aos.gyro_bias.r));
+
+ return TRUE;
+}
+
+
+
+
+
+int main ( int argc, char** argv) {
+
+ printf("hello\n");
+
+ g_timeout_add(1000/25, timeout_callback, NULL);
+
+ GMainLoop *ml = g_main_loop_new(NULL, FALSE);
+
+ IvyInit ("test_ahrs", "test_ahrs READY", NULL, NULL, NULL, NULL);
+ IvyStart("127.255.255.255");
+
+ imu_init();
+ ahrs_init();
+
+ aos_init();
+
+ g_main_loop_run(ml);
+
+ return 0;
+}
+
diff --git a/sw/airborne/test/pprz_algebra_print.h b/sw/airborne/test/pprz_algebra_print.h
index adbd26ab2a..2f67df9cc5 100644
--- a/sw/airborne/test/pprz_algebra_print.h
+++ b/sw/airborne/test/pprz_algebra_print.h
@@ -12,6 +12,10 @@
printf("%s %f %f %f\n",text, (_v).p, (_v).q, (_v).r); \
}
+#define DISPLAY_FLOAT_RATES_DEG(text, _v) { \
+ printf("%s %f %f %f\n",text, DegOfRad((_v).p), DegOfRad((_v).q), DegOfRad((_v).r)); \
+ }
+
#define DISPLAY_FLOAT_RMAT(text, mat) { \
printf("%s\n %f %f %f\n %f %f %f\n %f %f %f\n",text, \
mat.m[0], mat.m[1], mat.m[2], mat.m[3], mat.m[4], mat.m[5], \
@@ -70,6 +74,19 @@
printf("%s %d %d %d\n",text, (_v).p, (_v).q, (_v).r); \
}
+#define DISPLAY_INT32_RATES_AS_FLOAT(text, _r) { \
+ struct FloatRates _fr; \
+ RATES_FLOAT_OF_BFP(_fr, (_r)); \
+ DISPLAY_FLOAT_RATES(text, _fr); \
+ }
+
+#define DISPLAY_INT32_RATES_AS_FLOAT_DEG(text, _r) { \
+ struct FloatRates _fr; \
+ RATES_FLOAT_OF_BFP(_fr, (_r)); \
+ DISPLAY_FLOAT_RATES_DEG(text, _fr); \
+ }
+
+
#define DISPLAY_INT32_EULERS(text, _e) { \
printf("%s %d %d %d\n",text, (_e).phi, (_e).theta, (_e).psi); \
diff --git a/sw/airborne/test/subsystems/test_ahrs.c b/sw/airborne/test/subsystems/test_ahrs.c
new file mode 100644
index 0000000000..ac42d4b344
--- /dev/null
+++ b/sw/airborne/test/subsystems/test_ahrs.c
@@ -0,0 +1,191 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2011 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.
+ */
+
+#include
+
+#include "std.h"
+#include "mcu.h"
+#include "sys_time.h"
+#include "led.h"
+#include "mcu_periph/uart.h"
+#include "messages.h"
+#include "downlink.h"
+
+#include "subsystems/imu.h"
+#include "subsystems/ahrs.h"
+#include "subsystems/ahrs/ahrs_aligner.h"
+
+#include "my_debug_servo.h"
+
+static inline void main_init( void );
+static inline void main_periodic_task( void );
+static inline void main_event_task( void );
+static inline void main_report(void);
+
+static inline void on_gyro_event(void);
+static inline void on_accel_event(void);
+static inline void on_mag_event(void);
+
+int main( void ) {
+ main_init();
+ while(1) {
+ if (sys_time_periodic())
+ main_periodic_task();
+ main_event_task();
+ }
+ return 0;
+}
+
+static inline void main_init( void ) {
+
+ mcu_init();
+ sys_time_init();
+ imu_init();
+ ahrs_aligner_init();
+ ahrs_init();
+
+ DEBUG_SERVO1_INIT();
+ // DEBUG_SERVO2_INIT();
+
+ mcu_int_enable();
+}
+
+static inline void main_periodic_task( void ) {
+
+ if (cpu_time_sec > 1) imu_periodic();
+ RunOnceEvery(10, { LED_PERIODIC();});
+ main_report();
+}
+
+static inline void main_event_task( void ) {
+
+ ImuEvent(on_gyro_event, on_accel_event, on_mag_event);
+
+}
+
+static inline void on_gyro_event(void) {
+ ImuScaleGyro(imu);
+ if (ahrs.status == AHRS_UNINIT) {
+ ahrs_aligner_run();
+ if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED)
+ ahrs_align();
+ }
+ else {
+ DEBUG_S1_ON();
+ ahrs_propagate();
+ DEBUG_S1_OFF();
+ }
+}
+
+static inline void on_accel_event(void) {
+ ImuScaleAccel(imu);
+ if (ahrs.status != AHRS_UNINIT) {
+ DEBUG_S2_ON();
+ ahrs_update_accel();
+ DEBUG_S2_OFF();
+ }
+}
+
+static inline void on_mag_event(void) {
+ ImuScaleMag(imu);
+ if (ahrs.status == AHRS_RUNNING) {
+ ahrs_update_mag();
+ }
+}
+
+
+static inline void main_report(void) {
+
+ PeriodicPrescaleBy10(
+ {
+ DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,
+ &imu.accel_unscaled.x,
+ &imu.accel_unscaled.y,
+ &imu.accel_unscaled.z);
+ },
+ {
+ DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,
+ &imu.gyro_unscaled.p,
+ &imu.gyro_unscaled.q,
+ &imu.gyro_unscaled.r);
+ },
+ {
+ DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,
+ &imu.mag_unscaled.x,
+ &imu.mag_unscaled.y,
+ &imu.mag_unscaled.z);
+ },
+ {
+ DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel,
+ &imu.accel.x,
+ &imu.accel.y,
+ &imu.accel.z);
+ },
+ {
+ DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel,
+ &imu.gyro.p,
+ &imu.gyro.q,
+ &imu.gyro.r);
+ },
+
+ {
+ DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel,
+ &imu.mag.x,
+ &imu.mag.y,
+ &imu.mag.z);
+ },
+
+ {
+ DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
+ },
+ {
+#ifdef USE_I2C2
+ DOWNLINK_SEND_I2C_ERRORS(DefaultChannel,
+ &i2c2_errors.ack_fail_cnt,
+ &i2c2_errors.miss_start_stop_cnt,
+ &i2c2_errors.arb_lost_cnt,
+ &i2c2_errors.over_under_cnt,
+ &i2c2_errors.pec_recep_cnt,
+ &i2c2_errors.timeout_tlow_cnt,
+ &i2c2_errors.smbus_alert_cnt,
+ &i2c2_errors.unexpected_event_cnt,
+ &i2c2_errors.last_unexpected_event);
+#endif
+ },
+ {
+ DOWNLINK_SEND_BOOZ2_AHRS_EULER(DefaultChannel,
+ &ahrs.ltp_to_imu_euler.phi,
+ &ahrs.ltp_to_imu_euler.theta,
+ &ahrs.ltp_to_imu_euler.psi,
+ &ahrs.ltp_to_body_euler.phi,
+ &ahrs.ltp_to_body_euler.theta,
+ &ahrs.ltp_to_body_euler.psi);
+ },
+ {
+ DOWNLINK_SEND_BOOZ_AHRS_BIAS(DefaultChannel,
+ &ahrs_impl.gyro_bias.p,
+ &ahrs_impl.gyro_bias.q,
+ &ahrs_impl.gyro_bias.r);
+
+ });
+}
diff --git a/sw/airborne/test/subsystems/test_imu.c b/sw/airborne/test/subsystems/test_imu.c
new file mode 100644
index 0000000000..9591438b63
--- /dev/null
+++ b/sw/airborne/test/subsystems/test_imu.c
@@ -0,0 +1,159 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2011 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.
+ */
+
+#include
+
+#include "std.h"
+#include "mcu.h"
+#include "sys_time.h"
+#include "led.h"
+#include "mcu_periph/uart.h"
+#include "messages.h"
+#include "downlink.h"
+
+#include "subsystems/imu.h"
+
+#include "my_debug_servo.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_gyro_accel_event(void);
+static inline void on_accel_event(void);
+static inline void on_mag_event(void);
+
+int main( void ) {
+ main_init();
+ while(1) {
+ if (sys_time_periodic())
+ main_periodic_task();
+ main_event_task();
+ }
+ return 0;
+}
+
+static inline void main_init( void ) {
+
+ mcu_init();
+ sys_time_init();
+ imu_init();
+
+ // DEBUG_SERVO1_INIT();
+ // DEBUG_SERVO2_INIT();
+
+
+ mcu_int_enable();
+}
+
+static inline void main_periodic_task( void ) {
+ RunOnceEvery(100, {
+ // LED_TOGGLE(3);
+ DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
+ });
+#ifdef USE_I2C2
+ RunOnceEvery(111, {
+ DOWNLINK_SEND_I2C_ERRORS(DefaultChannel,
+ &i2c2_errors.ack_fail_cnt,
+ &i2c2_errors.miss_start_stop_cnt,
+ &i2c2_errors.arb_lost_cnt,
+ &i2c2_errors.over_under_cnt,
+ &i2c2_errors.pec_recep_cnt,
+ &i2c2_errors.timeout_tlow_cnt,
+ &i2c2_errors.smbus_alert_cnt,
+ &i2c2_errors.unexpected_event_cnt,
+ &i2c2_errors.last_unexpected_event);
+ });
+#endif
+ if (cpu_time_sec > 1) imu_periodic();
+ RunOnceEvery(10, { LED_PERIODIC();});
+}
+
+static inline void main_event_task( void ) {
+
+ ImuEvent(on_gyro_accel_event, on_accel_event, on_mag_event);
+
+}
+
+static inline void on_accel_event(void) {
+ ImuScaleAccel(imu);
+
+ static uint8_t cnt;
+ cnt++;
+ if (cnt > 15) cnt = 0;
+ if (cnt == 0) {
+ DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,
+ &imu.accel_unscaled.x,
+ &imu.accel_unscaled.y,
+ &imu.accel_unscaled.z);
+ }
+ else if (cnt == 7) {
+ DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel,
+ &imu.accel.x,
+ &imu.accel.y,
+ &imu.accel.z);
+ }
+}
+
+static inline void on_gyro_accel_event(void) {
+ ImuScaleGyro(imu);
+
+ static uint8_t cnt;
+ cnt++;
+ if (cnt > 15) cnt = 0;
+
+ if (cnt == 0) {
+ DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,
+ &imu.gyro_unscaled.p,
+ &imu.gyro_unscaled.q,
+ &imu.gyro_unscaled.r);
+ }
+ else if (cnt == 7) {
+ DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel,
+ &imu.gyro.p,
+ &imu.gyro.q,
+ &imu.gyro.r);
+ }
+}
+
+
+static inline void on_mag_event(void) {
+ ImuScaleMag(imu);
+ static uint8_t cnt;
+ cnt++;
+ if (cnt > 10) cnt = 0;
+
+ if (cnt == 0) {
+ DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel,
+ &imu.mag.x,
+ &imu.mag.y,
+ &imu.mag.z);
+ }
+ else if (cnt == 5) {
+ DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,
+ &imu.mag_unscaled.x,
+ &imu.mag_unscaled.y,
+ &imu.mag_unscaled.z);
+ }
+}
+
diff --git a/sw/airborne/test/subsystems/test_radio_control.c b/sw/airborne/test/subsystems/test_radio_control.c
new file mode 100644
index 0000000000..9daf3a9afc
--- /dev/null
+++ b/sw/airborne/test/subsystems/test_radio_control.c
@@ -0,0 +1,104 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2010 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.
+ */
+
+#include
+
+#include "mcu.h"
+#include "sys_time.h"
+#include "interrupt_hw.h"
+#include "mcu_periph/uart.h"
+
+#include "downlink.h"
+
+#include "subsystems/radio_control.h"
+
+static inline void main_init( void );
+static inline void main_periodic_task( void );
+static inline void main_event_task( void );
+static void main_on_radio_control_frame( void );
+//static void main_on_radio_control_status_changed( void );
+
+int main( void ) {
+ main_init();
+ while(1) {
+ if (sys_time_periodic())
+ main_periodic_task();
+ main_event_task();
+ }
+ return 0;
+}
+
+static inline void main_init( void ) {
+ mcu_init();
+ sys_time_init();
+ radio_control_init();
+ mcu_int_enable();
+}
+
+extern uint32_t debug_len;
+
+static inline void main_periodic_task( void ) {
+
+ RunOnceEvery(51, {
+ /*LED_TOGGLE(2);*/
+ uint32_t blaaa= cpu_time_sec;
+ DOWNLINK_SEND_TIME(DefaultChannel, &blaaa);
+ });
+
+ RunOnceEvery(10, {radio_control_periodic_task();});
+
+ int16_t foo = 0;//RC_PPM_SIGNED_TICS_OF_USEC(2050-1500);
+ RunOnceEvery(10,
+ {DOWNLINK_SEND_BOOZ2_RADIO_CONTROL(DefaultChannel, \
+ &radio_control.values[RADIO_ROLL], \
+ &radio_control.values[RADIO_PITCH], \
+ &radio_control.values[RADIO_YAW], \
+ &radio_control.values[RADIO_THROTTLE], \
+ &radio_control.values[RADIO_MODE], \
+ &foo, \
+ &radio_control.status);});
+#ifdef RADIO_CONTROL_TYPE_PPM
+ RunOnceEvery(10,
+ {uint8_t blaa = 0; DOWNLINK_SEND_PPM(DefaultChannel,&blaa, 8, booz_radio_control_ppm_pulses);});
+#endif
+
+ LED_PERIODIC();
+}
+
+static inline void main_event_task( void ) {
+
+ RadioControlEvent(main_on_radio_control_frame);
+
+}
+
+static void main_on_radio_control_frame( void ) {
+
+ // RunOnceEvery(10, {DOWNLINK_SEND_RC(RADIO_CONTROL_NB_CHANNEL, radio_control.values);});
+
+}
+
+/*
+static void main_on_radio_control_status_changed( void ) {
+
+}
+*/
diff --git a/sw/airborne/test/subsystems/test_settings.c b/sw/airborne/test/subsystems/test_settings.c
new file mode 100644
index 0000000000..1f33f4aad1
--- /dev/null
+++ b/sw/airborne/test/subsystems/test_settings.c
@@ -0,0 +1,110 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2011 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.
+ */
+#define DATALINK_C
+
+#include BOARD_CONFIG
+
+#include "std.h"
+#include "mcu.h"
+#include "sys_time.h"
+#include "downlink.h"
+#include "datalink.h"
+#include "subsystems/settings.h"
+
+#include "mcu_periph/uart.h"
+#include "messages.h"
+
+#include "my_debug_servo.h"
+
+static inline void main_init( void );
+static inline void main_periodic( void );
+static inline void main_event( void );
+
+
+float setting_a;
+float setting_b;
+float setting_c;
+float setting_d;
+
+int main( void ) {
+ main_init();
+ while(1) {
+ if (sys_time_periodic())
+ main_periodic();
+ main_event();
+ }
+ return 0;
+}
+
+
+static inline void main_init( void ) {
+
+ mcu_init();
+ sys_time_init();
+ settings_init();
+ // DEBUG_SERVO2_INIT();
+ // LED_ON(1);
+ // LED_ON(2);
+ // DEBUG_S4_ON();
+ // DEBUG_S5_ON();
+ // DEBUG_S6_ON();
+ mcu_int_enable();
+
+}
+
+static inline void main_periodic( void ) {
+
+ RunOnceEvery(100, {
+ DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
+ PeriodicSendDlValue(DefaultChannel);
+ });
+
+}
+
+static inline void main_event( void ) {
+
+ DatalinkEvent();
+
+}
+
+void dl_parse_msg(void) {
+ datalink_time = 0;
+ uint8_t msg_id = dl_buffer[1];
+ switch (msg_id) {
+
+ case DL_PING: {
+ DOWNLINK_SEND_PONG(DefaultChannel);
+ }
+ break;
+ case DL_SETTING:
+ if(DL_SETTING_ac_id(dl_buffer) == AC_ID) {
+ uint8_t i = DL_SETTING_index(dl_buffer);
+ float val = DL_SETTING_value(dl_buffer);
+ DlSetting(i, val);
+ DOWNLINK_SEND_DL_VALUE(DefaultChannel, &i, &val);
+ }
+ break;
+ default:
+ break;
+ }
+}
diff --git a/sw/airborne/test/subsystems/test_settings.h b/sw/airborne/test/subsystems/test_settings.h
new file mode 100644
index 0000000000..045893f992
--- /dev/null
+++ b/sw/airborne/test/subsystems/test_settings.h
@@ -0,0 +1,11 @@
+#ifndef TEST_SETTINGS_H
+#define TEST_SETTINGS_H
+
+#include "std.h"
+
+extern float setting_a;
+extern float setting_b;
+extern float setting_c;
+extern float setting_d;
+
+#endif
diff --git a/sw/airborne/test/test_algebra.c b/sw/airborne/test/test_algebra.c
index b8b7ce4d8b..ee67f3dffe 100644
--- a/sw/airborne/test/test_algebra.c
+++ b/sw/airborne/test/test_algebra.c
@@ -108,7 +108,7 @@ static void test_2(void) {
struct Int32Quat quat_i;
INT32_QUAT_OF_EULERS(quat_i, euler_i);
DISPLAY_INT32_QUAT("quat_i", quat_i);
- INT32_QUAT_NORMALISE(quat_i);
+ INT32_QUAT_NORMALIZE(quat_i);
DISPLAY_INT32_QUAT("quat_i_n", quat_i);
struct Int32Vect3 v2;
@@ -155,7 +155,7 @@ static void test_3(void) {
struct Int32Quat b2i_q;
INT32_QUAT_OF_EULERS(b2i_q, b2i_e);
DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q", b2i_q);
- // INT32_QUAT_NORMALISE(b2i_q);
+ // INT32_QUAT_NORMALIZE(b2i_q);
// DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q_n", b2i_q);
/* Compute BODY to IMU rotation matrix */
@@ -232,7 +232,7 @@ static void test_4_int(void) {
struct Int32Quat _q;
INT32_QUAT_OF_EULERS(_q, _e);
DISPLAY_INT32_QUAT_AS_EULERS_DEG("quat1 ", _q);
- // INT32_QUAT_NORMALISE(_q);
+ // INT32_QUAT_NORMALIZE(_q);
// DISPLAY_INT32_QUAT_2("_q_n", _q);
/* back to eulers */
@@ -257,7 +257,7 @@ static void test_4_float(void) {
struct FloatQuat q;
FLOAT_QUAT_OF_EULERS(q, e);
// DISPLAY_FLOAT_QUAT("q", q);
- FLOAT_QUAT_NORMALISE(q);
+ FLOAT_QUAT_NORMALIZE(q);
DISPLAY_FLOAT_QUAT("q_n", q);
DISPLAY_FLOAT_QUAT_AS_INT("q_n as int", q);
/* back to eulers */
diff --git a/sw/airborne/test/test_esc_mkk_simple.c b/sw/airborne/test/test_esc_mkk_simple.c
index 232890665a..983f19564b 100644
--- a/sw/airborne/test/test_esc_mkk_simple.c
+++ b/sw/airborne/test/test_esc_mkk_simple.c
@@ -59,7 +59,6 @@ static inline void main_periodic_task( void ) {
trans.buf[0] = 0x04;
trans.len_w = 1;
trans.slave_addr = 0x58;
- trans.stop_after_transmit = TRUE;
i2c_submit(&ACTUATORS_MKK_DEV,&trans);
LED_PERIODIC();
diff --git a/sw/airborne/test/test_geodetic.c b/sw/airborne/test/test_geodetic.c
index 4e017bdb30..774d353d1f 100644
--- a/sw/airborne/test/test_geodetic.c
+++ b/sw/airborne/test/test_geodetic.c
@@ -16,25 +16,24 @@
//#define DEBUG 1
-#define CM_OF_M(_m) ((_m)*1e2)
-#define M_OF_CM(_cm) ((_cm)/1e2)
-#define EM7RAD_OF_RAD(_r) (_r*1e7)
-#define RAD_OF_EM7RAD(_r) (_r/1e7)
-
+static void test_lla_of_utm(void);
static void test_floats(void);
static void test_doubles(void);
-static void test_enu_of_ecef_int(void);
+static void test_enu_of_ecef_int(void);
static void test_ned_to_ecef_to_ned(void);
static void test_enu_to_ecef_to_enu( void );
/*
- * toulouse 43.6052765, 1.4427764, 180.123019274324 -> 4624497.0 116475.0 4376563.0
+ * toulouse lat 43.6052765, lon 1.4427764, alt 180.123019274324 -> x 4624497.0 y 116475.0 z 4376563.0
*/
int main(int argc, char** argv) {
test_floats();
test_doubles();
+
+ test_lla_of_utm();
+
// test_enu_of_ecef_int();
// test_ned_to_ecef_to_ned();
@@ -43,6 +42,17 @@ int main(int argc, char** argv) {
}
+static void test_lla_of_utm(void) {
+ printf("\n--- lla of UTM double ---\n");
+
+ struct UTMCoor_d u = { .east=348805.71, .north=4759354.89, .zone=31 };
+ struct LlaCoor_d l;
+ struct LlaCoor_d l_ref = {.lat=0.749999999392454875,
+ .lon=0.019999999054505127};
+ lla_of_utm(&l, &u);
+ printf(" lat=%.16f lon=%.16f\nref_lat=%.16f ref_lon=%.16f\n",
+ l.lat, l.lon, l_ref.lat, l_ref.lon);
+}
static void test_floats(void) {
@@ -152,11 +162,11 @@ static void test_enu_of_ecef_int(void) {
M_OF_CM((double)my_enu_point_i.z));
#endif
- FLOAT_T ex = my_enu_point_f.x - M_OF_CM((double)my_enu_point_i.x);
+ float ex = my_enu_point_f.x - M_OF_CM((double)my_enu_point_i.x);
if (fabs(ex) > max_err.x) max_err.x = fabs(ex);
- FLOAT_T ey = my_enu_point_f.y - M_OF_CM((double)my_enu_point_i.y);
+ float ey = my_enu_point_f.y - M_OF_CM((double)my_enu_point_i.y);
if (fabs(ey) > max_err.y) max_err.y = fabs(ey);
- FLOAT_T ez = my_enu_point_f.z - M_OF_CM((double)my_enu_point_i.z);
+ float ez = my_enu_point_f.z - M_OF_CM((double)my_enu_point_i.z);
if (fabs(ez) > max_err.z) max_err.z = fabs(ez);
sum_err += ex*ex + ey*ey + ez*ez;
}
diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml
index 9c83aeea95..1f33553984 100644
--- a/sw/ground_segment/cockpit/live.ml
+++ b/sw/ground_segment/cockpit/live.ml
@@ -1158,11 +1158,11 @@ let listen_flight_params = fun geomap auto_center_new_ac alert alt_graph ->
let label = Pprz.string_assoc "ap_mode" vs in
ac.strip#set_label "AP" (if label="MANUAL" then "MANU" else label);
let color =
- match ap_mode with
- "AUTO2" -> ok_color
- | "AUTO1" -> "#10F0E0"
- | "MANUAL" -> warning_color
- | _ -> alert_color in
+ match ap_mode with
+ "AUTO2" | "NAV" -> ok_color
+ | "AUTO1" | "R_RCC" | "A_RCC" | "ATT_C" | "R_ZH" | "A_ZH" | "HOVER" | "HOV_C" | "H_ZH" -> "#10F0E0"
+ | "MANUAL" | "RATE" | "ATT" -> warning_color
+ | _ -> alert_color in
ac.strip#set_color "AP" color;
end;
let gps_mode = Pprz.string_assoc "gps_mode" vs in
diff --git a/sw/ground_segment/joystick/README b/sw/ground_segment/joystick/README
index 6c6fb0bde1..abc7bc77b5 100644
--- a/sw/ground_segment/joystick/README
+++ b/sw/ground_segment/joystick/README
@@ -21,7 +21,9 @@
#
-Tools for controling an aircraft with a ground joystick
+Tools for controlling an aircraft with a ground joystick
+
+Also used as general purpose joystick to ivybus connector. Can generate arbitrary ivybus messages based on control input from buttons or axes. Supports limiting, exponential and scaling on axes.
diff --git a/sw/ground_segment/joystick/input2ivy.ml b/sw/ground_segment/joystick/input2ivy.ml
index 926a5dc729..594275b14c 100644
--- a/sw/ground_segment/joystick/input2ivy.ml
+++ b/sw/ground_segment/joystick/input2ivy.ml
@@ -2,7 +2,7 @@
* $Id$
*
* Forwarding events from an USB input device to the ivy bus
- *
+ *
* Copyright (C) 2009 ENAC, Pascal Brisset
*
* This file is part of paparazzi.
@@ -20,16 +20,36 @@
* 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.
+ * Boston, MA 02111-1307, USA.
*
*)
+(* 1/26/2011 - Additional functionality added by jpeverill:
+ Joystick xml config files now loaded from PAPARAZZI_HOME/conf/joystick/
+ Exponential output setting (per channel)
+ Limit output setting (per channel)
+ Per channel trim, controlled through joystick buttons
+ Trim can also be saved into auxilliary file, based on aircraft, and loaded at runtime if it exists
+ File will be called ..trim and is saved in the conf/joystick directory as well
+ Division in channel mapping
+ Interactive keyboard trim control (primitive)
+*)
+
open Printf
+open Unix
+open Random
+
let (//) = Filename.concat
let conf_dir = Env.paparazzi_home // "conf"
let verbose = ref false (* Command line option *)
+(** global trim file name *)
+let trim_file_name = ref ""
+
+(** global joystick id *)
+let joystick_id = ref (Random.int 255)
+
(** Messages libraries *)
module DL = Pprz.Messages(struct let name = "datalink" end)
module G = Pprz.Messages(struct let name = "ground" end)
@@ -55,10 +75,11 @@ external stick_read : unit -> int * int * int array = "ml_stick_read"
(** Range for the input axis *)
let max_input = 127
let min_input = -127
+let trim_step = 0.2
(** Representation of an input value *)
type input =
- Axis of int * int (* (index, deadband) *)
+ Axis of int * int * float * float * float ref (* (index, deadband, limit, exponent, trim ) *)
| Button of int
(** Description of a message *)
@@ -66,17 +87,40 @@ type msg = {
msg_name : string;
msg_class : string;
fields : (string * Syntax.expression) list;
- on_event : Syntax.expression option
+ on_event : Syntax.expression option;
+ send_always : bool
}
-(** Represenation of an input device and of the messages to send *)
+(** Representation of a variable *)
+type var = {
+ mutable value : int;
+ var_event : (int * Syntax.expression) list;
+}
+
+(** Represenation of an input device, the messages to send and the variables *)
type actions = {
period_ms : int;
inputs : (string*input) list;
- messages : msg list
+ messages : msg list;
+ variables : (string*var) list;
}
+(** adjust the trim on an axis given its name *)
+let trim_adjust = fun axis_name adjustment ->
+ None
+
+(** Get message class type *)
+let get_message_type = fun class_name ->
+ match class_name with
+ "datalink" -> "Message"
+ | "ground" -> "Message"
+ | "trim_plus" -> "Trim"
+ | "trim_minus" -> "Trim"
+ | "trim_save" -> "Trim"
+ | _ -> failwith class_name
+
(** Get a message description from its name (and class name) *)
+(** class_names with entries above as "Message" should be listed here *)
let get_message = fun class_name msg_name ->
match class_name with
"datalink" -> snd (DL.message_of_name msg_name)
@@ -117,7 +161,7 @@ let hash_index_of_blocks = fun ac_name ->
List.iter (fun block ->
Hashtbl.add
index_of_blocks
- (Xml.attrib block "name")
+ (Xml.attrib block "name")
(ExtXml.int_attrib block "no"))
(Xml.children blocks)
@@ -135,15 +179,15 @@ let eval_settings_and_blocks = fun field_descr expr ->
let rec loop = function
Syntax.Call ("IndexOfEnum", [Syntax.Ident enum]) -> begin
try Syntax.Int (rank enum field_descr.Pprz.enum) with
- Not_found -> failwith (sprintf "IndexOfEnum: unknown value '%s'" enum)
+ Not_found -> failwith (sprintf "IndexOfEnum: unknown value '%s'" enum)
end
| Syntax.Call ("IndexOfSetting", [Syntax.Ident var]) -> begin
try Syntax.Int (Hashtbl.find index_of_settings var) with
- Not_found -> failwith (sprintf "IndexOfSetting: unknown var '%s'" var)
+ Not_found -> failwith (sprintf "IndexOfSetting: unknown var '%s'" var)
end
| Syntax.Call ("IndexOfBlock", [Syntax.Ident name]) -> begin
try Syntax.Int (Hashtbl.find index_of_blocks name) with
- Not_found -> failwith (sprintf "IndexOfBlock: unknown block '%s'" name)
+ Not_found -> failwith (sprintf "IndexOfBlock: unknown block '%s'" name)
end
| Syntax.Call (ident, exprs) | Syntax.CallOperator (ident, exprs) ->
Syntax.Call (ident, List.map loop exprs)
@@ -152,19 +196,21 @@ let eval_settings_and_blocks = fun field_descr expr ->
(** Parse an XML list of input channels *)
let parse_input = fun input ->
- List.map
- (fun x ->
- let name = Xml.attrib x "name"
- and index = ExtXml.int_attrib x "index" in
- let value =
- match Xml.tag x with
- "axis" ->
- let deadband = try ExtXml.int_attrib x "deadband" with _ -> 0 in
- Axis (index, deadband)
- | "button" -> Button index
- | _ -> failwith "parse_input: unexepcted tag" in
- (name, value))
- (Xml.children input)
+ List.map (fun x ->
+ let name = Xml.attrib x "name"
+ and index = ExtXml.int_attrib x "index" in
+ let value =
+ match Xml.tag x with
+ "axis" ->
+ let trim = try ExtXml.float_attrib x "trim" with _ -> 0.0 in
+ let exponent = try ExtXml.float_attrib x "exponent" with _ -> 0.0 in
+ let limit = try ExtXml.float_attrib x "limit" with _ -> 1.0 in
+ let deadband = try ExtXml.int_attrib x "deadband" with _ -> 0 in
+ Axis (index, deadband, limit, exponent, ref trim)
+ | "button" -> Button index
+ | _ -> failwith "parse_input: unexepcted tag" in
+ (name, value))
+ (Xml.children input)
(** Parse a 'à la C' expression *)
let parse_value = fun s ->
@@ -181,11 +227,16 @@ let parse_msg_field = fun msg_descr field ->
(** Parse a complete message and build its representation *)
let parse_msg = fun msg ->
let msg_name = Xml.attrib msg "name"
- and msg_class = Xml.attrib msg "class" in
+ and msg_class = Xml.attrib msg "class"
+ and send_always = (try (Xml.attrib msg "send_always") = "true" with _ -> false) in
- let msg_descr = get_message msg_class msg_name in
-
- let fields = List.map (parse_msg_field msg_descr) (Xml.children msg) in
+ let fields =
+ match get_message_type msg_class with
+ "Message" ->
+ let msg_descr = get_message msg_class msg_name in
+ List.map (parse_msg_field msg_descr) (Xml.children msg)
+ | "Trim" -> []
+ | _ -> failwith ("Unknown message class type") in
let on_event =
try Some (parse_value (Xml.attrib msg "on_event")) with _ -> None in
@@ -193,64 +244,171 @@ let parse_msg = fun msg ->
{ msg_name = msg_name;
msg_class = msg_class;
fields = fields;
- on_event = on_event
+ on_event = on_event;
+ send_always = send_always
}
-(** Parse the complete (input and messages) XML desxription *)
-let parse_descr = fun xml_file ->
- let xml = Xml.parse_file xml_file in
+(** Parse an XML list of variables and set function *)
+let parse_variables = fun variables ->
+ let l = ref [] in
+ List.iter (fun x ->
+ match Xml.tag x with
+ "var" ->
+ let name = Xml.attrib x "name"
+ and default = ExtXml.int_attrib x "default" in
+ if List.mem_assoc name !l then failwith (sprintf "Variable %s already declared" name);
+ (* filter all "set" node for this variable *)
+ let set = List.filter (fun vs ->
+ (ExtXml.tag_is vs "set") &&
+ (compare (ExtXml.attrib_or_default vs "var" "") name) = 0)
+ (Xml.children variables) in
+ let var_event = List.map (fun s ->
+ let value = ExtXml.int_attrib s "value"
+ and on_event = parse_value (Xml.attrib s "on_event") in
+ (value, on_event)
+ ) set in
+ l := (name, { value = default; var_event = var_event; }) :: !l;
+ ()
+ | _ -> ()
+ ) (Xml.children variables);
+ !l
- let inputs = parse_input (ExtXml.child xml "input")
- and messages_xml = ExtXml.child xml "messages" in
- let period_ms =truncate (1000.*.ExtXml.float_attrib messages_xml "period")
- and messages = List.map parse_msg (Xml.children messages_xml) in
-
- { period_ms = period_ms; inputs = inputs; messages = messages }
-
-(** Verbose List.assco *)
+(** Verbose List.assoc *)
let my_assoc = fun x l ->
try List.assoc x l with Not_found ->
failwith (sprintf "my_assoc: %s not found" x)
+let first_of_two (x,_) = x
+
+let second_of_two (_,x) = x
+
+(** set a trim value given an inputs array and a trim_values tuple *)
+let trim_set = fun inputs value ->
+ let input = my_assoc (first_of_two value) inputs in
+ match input with
+ Axis (i, deadband, limit, exponent, trim) -> trim := (second_of_two value)
+ | Button i -> failwith "No trim for buttons"
+
+
+(** Input the trim file if it exists *)
+let parse_trim_file = fun trim_file_name inputs ->
+ if Sys.file_exists trim_file_name then begin
+ let trim = Xml.parse_file trim_file_name in
+ let trim_values = List.map
+ (fun x ->
+ let axis = ExtXml.attrib x "axis"
+ and trimval = ExtXml.float_attrib x "value" in
+ (axis, trimval))
+ (Xml.children trim) in
+ List.iter (trim_set inputs) trim_values;
+ end
+
+(** Parse the complete (input and messages) XML desxription
+ Also parses the trim xml file if it exists *)
+let parse_descr = fun xml_file trim_file ->
+ let xml = Xml.parse_file xml_file in
+
+ let inputs = parse_input (ExtXml.child xml "input")
+ and messages_xml = ExtXml.child xml "messages"
+ and variables = try parse_variables (ExtXml.child xml "variables") with _ -> [] in
+
+ let period_ms = int_of_float (1000.*.ExtXml.float_attrib messages_xml "period")
+ and messages = List.map parse_msg (Xml.children messages_xml) in
+
+ (** check for trim file *)
+ parse_trim_file trim_file inputs;
+
+ { period_ms = period_ms; inputs = inputs; messages = messages; variables = variables }
+
+(** apply deadband - applied first *)
let apply_deadband = fun x min ->
if abs x < min then 0 else x
+(** apply limit - applied third *)
+let apply_limit = fun x limit ->
+ limit *. x
+
+(** apply exponent - applied second *)
+let apply_exponent = fun x expon ->
+ let pow_value = (float_of_int x) ** 3. /. (float_of_int max_input) ** 2. in
+ ( (float_of_int x) *. (1. -. expon)) +. (pow_value *. expon)
+
+(** apply trim - applied fourth *)
+let apply_trim = fun x trim ->
+ let x_new = (int_of_float (x +. trim)) in
+ if x_new > max_input then max_input else (if x_new < min_input then min_input else x_new)
+
(** Access to an input value, button or axis *)
let eval_input = fun buttons axis input ->
match input with
- Axis (i, deadband) -> apply_deadband axis.(i) deadband
+ Axis (i, deadband, limit, exponent, trim) -> (apply_trim (apply_limit (apply_exponent (apply_deadband axis.(i) deadband) exponent) limit) trim.contents)
| Button i -> (buttons lsr i) land 0x1
(** Scale a value in the given bounds *)
let scale = fun x min max ->
min + ((x - min_input) * (max - min)) / (max_input - min_input)
+(** Fit a given interval of value into [min_input; max_input] *)
+let fit = fun x min max min_input max_input ->
+ min_input + ((x - min) * (max_input - min_input)) / (max - min)
+
+(** Scale a value in the given bounds *)
+let bound = fun x min max ->
+ if x < min then min else (if x > max then max else x)
+
+(** Return a pprz RC mode
+ * mode > max -> 2
+ * mode < min -> 0
+ * else 1
+ *)
+let pprz_threshold = max_input / 2
+let pprz_mode = fun mode ->
+ if mode > pprz_threshold then 2
+ else if mode < -pprz_threshold then 0
+ else 1
+
(** Eval a function call (TO BE COMPLETED) *)
let eval_call = fun f args ->
match f, args with
"-", [a1; a2] -> a1 - a2
| "+", [a1; a2] -> a1 + a2
| "*", [a1; a2] -> a1 * a2
+ | "%", [a1; a2] -> a1 / a2
| "&&", [a1; a2] -> a1 land a2
+ | "||", [a1; a2] -> a1 lor a2
+ | "<", [a1; a2] -> if a1 < a2 then 1 else 0
+ | ">", [a1; a2] -> if a1 > a2 then 1 else 0
| "Scale", [x; min; max] -> scale (x) (min) (max)
+ | "Fit", [x; min; max; min_input; max_input] -> fit (x) (min) (max) (min_input) (max_input)
+ | "Bound", [x; min; max] -> bound (x) (min) (max)
+ | "PprzMode", [x] -> pprz_mode (x)
+ | "JoystickID", [] -> !joystick_id
| f, args -> failwith (sprintf "eval_call: unknown function '%s'" f)
(** Eval an expression *)
-let eval_expr = fun buttons axis inputs expr ->
+let eval_expr = fun buttons axis inputs variables expr ->
let rec eval = function
Syntax.Ident ident ->
- let input = my_assoc ident inputs in
- eval_input buttons axis input
+ (* try input first, then variables *)
+ let i = match (List.mem_assoc ident inputs, List.mem_assoc ident variables) with
+ (true, _) -> eval_input buttons axis (List.assoc ident inputs)
+ | (false, true) ->
+ let v = List.assoc ident variables in
+ v.value
+ | (false, false) -> failwith (sprintf "eval_expr: %s not found" ident)
+ in
+ i
| Syntax.Int int -> int
| Syntax.Float float -> failwith "eval_expr: float"
| Syntax.Call (ident, exprs) | Syntax.CallOperator (ident, exprs) ->
- eval_call ident (List.map eval exprs)
+ eval_call ident (List.map eval exprs)
| Syntax.Index _ -> failwith "eval_expr: index"
- | Syntax.Field _ -> failwith "eval_expr: Field" in
+ | Syntax.Field _ -> failwith "eval_expr: Field"
+ | Syntax.Deref _ -> failwith "eval_expr: deref" in
eval expr
-
+
(** Store for the last sent values: msg_name->values *)
let last_values = Hashtbl.create 5
@@ -262,24 +420,69 @@ let get_previous_values = fun msg_name ->
let record_values = fun msg_name values ->
Hashtbl.replace last_values msg_name values
-(**Send an ivy message if needed: new values and/or 'on_event' condition true*)
-let execute_action = fun ac_id inputs buttons axis message ->
+let second_list (_,x) = x
+let first_list (x,_) = x
+
+(** add a leaf *)
+let trim_save_add_leaf = fun x channel_pair ->
+ let chan_name = first_list channel_pair in
+ let channel = second_list channel_pair in
+ match channel with
+ Axis (i, deadband, limit, exponent, trim) -> x := x.contents ^ (Printf.sprintf " " chan_name trim.contents)
+ | Button i -> Printf.printf "%d" i
+
+(** save trim settings to file *)
+let trim_save = fun inputs ->
+ let xmlstring = ref "" in
+ List.iter (trim_save_add_leaf xmlstring) inputs;
+ xmlstring := xmlstring.contents ^ " ";
+ let x = Xml.parse_string xmlstring.contents in
+ let pretty_xml_string = Xml.to_string_fmt x in
+ let output_trim_file = open_out trim_file_name.contents in
+ output_string output_trim_file pretty_xml_string;
+ close_out output_trim_file
+
+(** Adjust the trim on a specified channel *)
+let trim_adjust = fun axis_name adjustment inputs ->
+ let input = my_assoc axis_name inputs in
+ match input with
+ Axis (i, deadband, limit, exponent, trim) -> trim := trim.contents +. adjustment
+ | Button i -> failwith "No trim for buttons"
+
+(** Update variables state *)
+let update_variables = fun inputs buttons axis variables ->
+ List.iter (fun (_,var) ->
+ List.iter (fun (value, expr) ->
+ let event = eval_expr buttons axis inputs variables expr in
+ if event <> 0 then begin
+ (* remove and add again ? *)
+ var.value <- value
+ end
+ ) var.var_event
+ ) variables
+
+(** Send an ivy message if needed: new values and/or 'on_event' condition true*)
+let execute_action = fun ac_id inputs buttons axis variables message ->
let values =
List.map
- (fun (name, expr) -> (name, Pprz.Int (eval_expr buttons axis inputs expr)))
+ (fun (name, expr) -> (name, Pprz.Int (eval_expr buttons axis inputs variables expr)))
message.fields
- and on_event =
+ and on_event =
match message.on_event with
None -> true
- | Some expr -> eval_expr buttons axis inputs expr <> 0 in
+ | Some expr -> eval_expr buttons axis inputs variables expr <> 0 in
let previous_values = get_previous_values message.msg_name in
- if (on_event, values) <> previous_values && on_event then begin
+ (* FIXME ((value <> previous) && on_event) || send_always ??? *)
+ if ( ( (on_event, values) <> previous_values ) || message.send_always ) && on_event then begin
let vs = ("ac_id", Pprz.Int ac_id) :: values in
match message.msg_class with
"datalink" -> DL.message_send "input2ivy" message.msg_name vs
| "ground" -> G.message_send "input2ivy" message.msg_name vs
+ | "trim_plus" -> trim_adjust message.msg_name trim_step inputs
+ | "trim_minus" -> trim_adjust message.msg_name (-.trim_step) inputs
+ | "trim_save" -> trim_save inputs
| c -> failwith (sprintf "execute_action: unknown class '%s'" c)
end;
record_values message.msg_name (on_event, values)
@@ -287,44 +490,73 @@ let execute_action = fun ac_id inputs buttons axis message ->
(** Output on stderr the values from the input device *)
let print_inputs = fun nb_buttons buttons axis ->
- fprintf stderr "buttons: ";
+ fprintf Pervasives.stderr "buttons: ";
for i = 0 to nb_buttons - 1 do
- fprintf stderr "%d:%d " i (eval_input buttons axis (Button i))
+ fprintf Pervasives.stderr "%d:%d " i (eval_input buttons axis (Button i))
done;
- fprintf stderr "\naxis: ";
+ fprintf Pervasives.stderr "\naxis: ";
for i = 0 to Array.length axis - 1 do
- fprintf stderr "%d:%d " i (eval_input buttons axis (Axis (i, 0)))
+ fprintf Pervasives.stderr "%d:%d " i (eval_input buttons axis (Axis (i, 0, 1.0, 0.0, ref 0.0)))
done;
- fprintf stderr "\n%!"
-
+ fprintf Pervasives.stderr "\n%!"
-(** Get the values from the input values and send messages *)
+
+(** Get the values from the input values and send messages
+ This is called at a rate programmed in the xml *)
let execute_actions = fun actions ac_id ->
try
let (nb_buttons, buttons, axis) = stick_read () in
if !verbose then
print_inputs nb_buttons buttons axis;
-
- List.iter (execute_action ac_id actions.inputs buttons axis) actions.messages
+
+ (* TODO update variables before msg *)
+ update_variables actions.inputs buttons axis actions.variables;
+ List.iter (execute_action ac_id actions.inputs buttons axis actions.variables) actions.messages
with
exc -> prerr_endline (Printexc.to_string exc)
+(** process keyboard commands *)
+(** used for adjusting trims interactively from the keyboard *)
+(** this capability is mostly for bench-time trimming when a joystick does not have adequate buttons *)
+(** it is not a very complete capability *)
+let execute_kb_action = fun actions conditions ->
+ let ch = input_byte Pervasives.stdin in
+ (** esdx for left stick
+ ijkm for right *)
+
+ if true then begin
+ match ch with
+ 101 -> trim_adjust "ly" 1.0 actions.inputs
+ | 115 -> trim_adjust "lx" (-1.0) actions.inputs
+ | 100 -> trim_adjust "lx" 1.0 actions.inputs
+ | 120 -> trim_adjust "ly" (-1.0) actions.inputs
+ | 105 -> trim_adjust "ry" 1.0 actions.inputs
+ | 106 -> trim_adjust "rx" (-1.0) actions.inputs
+ | 107 -> trim_adjust "rx" 1.0 actions.inputs
+ | 109 -> trim_adjust "ry" (-1.0) actions.inputs
+ | _ -> trim_adjust "ly" 0.0 actions.inputs
+ end;
+
+ true
+
+
(************************************* MAIN **********************************)
let () =
-let ivy_bus = Defivybus.default_ivy_bus in
- let device_name = ref ""
+ let ivy_bus = ref Defivybus.default_ivy_bus in
+ let device_name = ref "/dev/input/js0"
and ac_name = ref "MYAC"
and xml_descr = ref "" in
let anon_fun = (fun x -> xml_descr := x) in
- let speclist =
+ let speclist =
[ "-b", Arg.String (fun x -> ivy_bus := x),(sprintf " Default is %s" !ivy_bus);
"-ac", Arg.Set_string ac_name, "";
"-d", Arg.Set_string device_name, "";
"-v", Arg.Set verbose, "Verbose mode (useful to identify the channels of an input device)";
+ "-id", Arg.Set_int joystick_id, "Joystick ID, from 0-255. Each joystick requires a unique ID in a multiple joystick configuration.";
"-", Arg.String anon_fun, ""
]
and usage_msg = "Usage: " in
@@ -341,7 +573,13 @@ let ivy_bus = Defivybus.default_ivy_bus in
hash_index_of_settings !ac_name;
hash_index_of_blocks !ac_name;
- let actions = parse_descr !xml_descr in
+ Printf.printf "Joystick ID: %u\n" !joystick_id;
+
+ let joystick_conf_dir = conf_dir ^ "/joystick/" in
+ let xml_descr_full = joystick_conf_dir ^ !xml_descr in
+ trim_file_name := String.concat "." [xml_descr_full ; !ac_name ; "trim"];
+
+ let actions = parse_descr xml_descr_full trim_file_name.contents in
if stick_init !device_name <> 0 then
failwith (sprintf "Error: cannot open device %s\n" !device_name);
@@ -350,8 +588,14 @@ let ivy_bus = Defivybus.default_ivy_bus in
Ivy.init "Paparazzi joystick" "READY" (fun _ _ -> ());
Ivy.start !ivy_bus;
+ (** setup stdin *)
+ let tstatus = (Unix.tcgetattr Unix.stdin) in
+ tstatus.c_icanon <- false;
+ Unix.tcsetattr Unix.stdin Unix.TCSANOW tstatus;
+
ignore (Glib.Timeout.add actions.period_ms (fun () -> execute_actions actions ac_id; true));
-
+ ignore (Glib.Io.add_watch ~cond:[`IN] ~callback:(fun x -> execute_kb_action actions x) (Glib.Io.channel_of_descr Unix.stdin));
+
(** Start the main loop *)
let loop = Glib.Main.create true in
while Glib.Main.is_running loop do ignore (Glib.Main.iteration true) done
diff --git a/sw/ground_segment/joystick/usb_stick.c b/sw/ground_segment/joystick/usb_stick.c
index 87c7a3fab8..8d78c8b316 100644
--- a/sw/ground_segment/joystick/usb_stick.c
+++ b/sw/ground_segment/joystick/usb_stick.c
@@ -21,6 +21,11 @@
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
+/* 1/26/2011 - jpeverill added
+ support for joystick specific event interface. During testing it was found that many devices do not work properly with the standard event interface (such as the PS3 joystick). The joystick interface seems to work better. The old event interface is used if "event" is found in the name, otherwise the joystick interface is used.
+ now kills the process in the event of a read error (joystick unplugs)
+*/
+
//#define STICK_DBG 1
#include "usb_stick.h"
@@ -33,11 +38,14 @@
#include
#include
#include
-#include
#include
#include
#include
+//needed for joystick interface
+#include
+#include
+
#ifdef STICK_DBG
#define dbgprintf fprintf
#else
@@ -62,15 +70,19 @@
int stick_device_handle;
int8_t stick_axis_values[AXIS_COUNT] = {0, 0, 0, 0, 0, 0};
-int16_t stick_button_values = 0;
+int32_t stick_button_values = 0;
int axis_code[AXIS_COUNT];
int stick_axis_count = 0;
int button_code[BUTTON_COUNT];
int stick_button_count = 0;
+int hid_led_count = 0;
+
int32_t axis_min[AXIS_COUNT], axis_max[AXIS_COUNT];
+int event_mode = STICK_MODE_UNKNOWN;
+
struct stick_code_param_ stick_init_param = {
0, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
0, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
@@ -79,17 +91,56 @@ struct stick_code_param_ stick_init_param = {
//struct ff_effect effect;
+void led_test( void ) {
+
+ struct input_event ev; /* the event */
+
+ int lednum;
+
+ /* we turn off all the LEDs to start */
+ ev.type = EV_LED;
+
+ while (1) {
+ for (lednum=0;lednum<32;lednum++) {
+ ev.code = lednum;
+ ev.value = 1;
+ write(stick_device_handle, &ev, sizeof(struct input_event));
+ }
+
+ usleep(1000000);
+
+ for (lednum=0;lednum<32;lednum++) {
+ ev.code = lednum;
+ ev.value = 0;
+ write(stick_device_handle, &ev, sizeof(struct input_event));
+ }
+ }
+
+
+}
+
+
int init_hid_device(char* device_name)
{
int cnt;
- unsigned long key_bits[32],abs_bits[32];
+ unsigned long buttons,axes;
+ unsigned long key_bits[32],abs_bits[32],led_bits[32];
// unsigned long ff_bits[32];
int valbuf[16];
- char name[256] = "Unknown";
+ char name[MAX_NAME_LENGTH] = "Unknown";
stick_button_count = 0;
stick_axis_count = 0;
+ /* Detect device mode, event interface or joystick */
+ if (strstr( device_name, "event" ) == 0 ) {
+ dbgprintf(stderr," Using Joystick interface\n");
+ event_mode = STICK_MODE_JOYSTICK;
+ } else {
+ dbgprintf(stderr," Using Event interface\n");
+ event_mode = STICK_MODE_EVENT;
+ }
+
/* Open event device read only (with write permission for ff) */
stick_device_handle = open(device_name,O_RDONLY|O_NONBLOCK);
if (stick_device_handle<0) {
@@ -98,12 +149,35 @@ int init_hid_device(char* device_name)
return(1);
}
+ /* LED testing */
+
+// led_test( );
+
+
/* Which buttons has the device? */
memset(key_bits,0,32*sizeof(unsigned long));
- if (ioctl(stick_device_handle,EVIOCGBIT(EV_KEY,32*sizeof(unsigned long)),key_bits)<0) {
- dbgprintf(stderr,"ERROR: can not get key bits (%s) [%s:%d]\n",
- strerror(errno),__FILE__,__LINE__);
- return(1);
+ memset(led_bits,0,32*sizeof(unsigned long));
+ if (event_mode == STICK_MODE_EVENT ) {
+ if (ioctl(stick_device_handle,EVIOCGBIT(EV_KEY,32*sizeof(unsigned long)),key_bits)<0) {
+ dbgprintf(stderr,"ERROR: can not get key bits (%s) [%s:%d]\n",
+ strerror(errno),__FILE__,__LINE__);
+ return(1);
+ }
+ //read LED count
+ if (ioctl(stick_device_handle,EVIOCGLED(32*sizeof(led_bits)),led_bits)<0) {
+ dbgprintf(stderr,"ERROR: can not get LED bits (%s) [%s:%d]\n",
+ strerror(errno),__FILE__,__LINE__);
+ return(1);
+ }
+ dbgprintf(stderr,"LEDs: %X",*led_bits);
+
+
+ } else {
+ if (ioctl(stick_device_handle,JSIOCGBUTTONS,&buttons)<0) {
+ dbgprintf(stderr,"ERROR: can not get key bits (%s) [%s:%d]\n",
+ strerror(errno),__FILE__,__LINE__);
+ return(1);
+ }
}
/* Store buttons */
@@ -118,13 +192,22 @@ int init_hid_device(char* device_name)
memcpy(button_code,stick_init_param.button_code,BUTTON_COUNT*sizeof(int));
}
else {
- for (cnt = MIN_BUTTON_CODE; cnt < MAX_BUTTON_CODE; cnt++) {
- if (TEST_BIT(cnt, key_bits)) {
+ if (event_mode == STICK_MODE_EVENT) {
+ for (cnt = MIN_BUTTON_CODE; cnt < MAX_BUTTON_CODE; cnt++) {
+ if (TEST_BIT(cnt, key_bits)) {
+ button_code[stick_button_count++] = cnt;
+ dbgprintf(stderr,"Available button: %d (0x%x)\n",cnt,cnt);
+ }
+ if (stick_button_count == BUTTON_COUNT) break;
+ }
+ } else {
+ for (cnt = 0; cnt < buttons; cnt++) {
button_code[stick_button_count++] = cnt;
dbgprintf(stderr,"Available button: %d (0x%x)\n",cnt,cnt);
+ if (stick_button_count == BUTTON_COUNT) break;
}
- if (stick_button_count == BUTTON_COUNT) break;
}
+
if (stick_button_count == 0) {
dbgprintf(stderr,"ERROR: no suitable buttons found [%s:%d]\n",__FILE__,__LINE__);
}
@@ -132,10 +215,18 @@ int init_hid_device(char* device_name)
/* Which axis has the device? */
memset(abs_bits,0,32*sizeof(unsigned long));
- if (ioctl(stick_device_handle,EVIOCGBIT(EV_ABS,32*sizeof(unsigned long)),abs_bits)<0) {
- dbgprintf(stderr,"ERROR: can not get abs bits (%s) [%s:%d]\n",
- strerror(errno),__FILE__,__LINE__);
- return(1);
+ if (event_mode == STICK_MODE_EVENT ) {
+ if (ioctl(stick_device_handle,EVIOCGBIT(EV_ABS,32*sizeof(unsigned long)),abs_bits)<0) {
+ dbgprintf(stderr,"ERROR: can not get abs bits (%s) [%s:%d]\n",
+ strerror(errno),__FILE__,__LINE__);
+ return(1);
+ }
+ } else {
+ if (ioctl(stick_device_handle,JSIOCGAXES,&axes)<0) {
+ dbgprintf(stderr,"ERROR: can not get abs bits (%s) [%s:%d]\n",
+ strerror(errno),__FILE__,__LINE__);
+ return(1);
+ }
}
/* Store axis */
@@ -150,13 +241,22 @@ int init_hid_device(char* device_name)
memcpy(axis_code,stick_init_param.axis_code,AXIS_COUNT*sizeof(int));
}
else {
- for (cnt = MIN_ABS_CODE; cnt < MAX_ABS_CODE; cnt++) {
- if (TEST_BIT(cnt, abs_bits)) {
+ if(event_mode == STICK_MODE_EVENT) {
+ for (cnt = MIN_ABS_CODE; cnt < MAX_ABS_CODE; cnt++) {
+ if (TEST_BIT(cnt, abs_bits)) {
+ axis_code[stick_axis_count++] = cnt;
+ dbgprintf(stderr,"Available axis: %d (0x%x)\n",cnt,cnt);
+ }
+ if (stick_axis_count == AXIS_COUNT) break;
+ }
+ } else {
+ for (cnt = 0; cnt < axes; cnt++) {
axis_code[stick_axis_count++] = cnt;
dbgprintf(stderr,"Available axis: %d (0x%x)\n",cnt,cnt);
+ if (stick_axis_count == AXIS_COUNT) break;
}
- if (stick_axis_count == AXIS_COUNT) break;
}
+
// at least 2 axis are needed in auto detection
if (stick_axis_count < 2) {
dbgprintf(stderr,"ERROR: no suitable axis found [%s:%d]\n",__FILE__,__LINE__);
@@ -167,14 +267,21 @@ int init_hid_device(char* device_name)
/* Axis param */
for (cnt = 0; cnt < stick_axis_count; cnt++)
{
- /* get axis value range */
- if (ioctl(stick_device_handle,EVIOCGABS(axis_code[cnt]),valbuf)<0) {
- dbgprintf(stderr,"ERROR: can not get axis %d value range (%s) [%s:%d]\n",
- cnt,strerror(errno),__FILE__,__LINE__);
- return(1);
- }
- axis_min[cnt]=valbuf[1];
- axis_max[cnt]=valbuf[2];
+ if (event_mode == STICK_MODE_EVENT ) {
+ /* get axis value range */
+ if (ioctl(stick_device_handle,EVIOCGABS(axis_code[cnt]),valbuf)<0) {
+ dbgprintf(stderr,"ERROR: can not get axis %d value range (%s) [%s:%d]\n",
+ cnt,strerror(errno),__FILE__,__LINE__);
+ return(1);
+ }
+ axis_min[cnt]=valbuf[1];
+ axis_max[cnt]=valbuf[2];
+ } else {
+ // with joystick interface, all axes are signed 16 bit with full range
+ axis_min[cnt]=-32768;
+ axis_max[cnt]=32768;
+ }
+
if (axis_min[cnt]>=axis_max[cnt]) {
dbgprintf(stderr,"ERROR: bad axis %d value range (%d,%d) [%s:%d]\n",
cnt,axis_min[cnt],axis_max[cnt],__FILE__,__LINE__);
@@ -184,6 +291,9 @@ int init_hid_device(char* device_name)
cnt,axis_min[cnt],axis_max[cnt]);
}
+ //--------------------------------------------------
+ // force feedback, TBD feature
+ //--------------------------------------------------
#if 0
/* Now get some information about force feedback */
memset(ff_bits,0,32*sizeof(unsigned long));
@@ -245,7 +355,11 @@ int init_hid_device(char* device_name)
}
#endif
- ioctl(stick_device_handle, EVIOCGNAME(sizeof(name)), name);
+ if (event_mode == STICK_MODE_EVENT ) {
+ ioctl(stick_device_handle, EVIOCGNAME(sizeof(name)), name);
+ } else {
+ ioctl(stick_device_handle, JSIOCGNAME(MAX_NAME_LENGTH), name);
+ }
printf("Input device name: \"%s\" on device \"%s\"\n", name, device_name);
return(0);
@@ -256,61 +370,79 @@ int stick_read( void ) {
int cnt;
struct input_event event;
+ struct js_event jsevent;
- /* Get events */
- while (read(stick_device_handle,&event,sizeof(event))==sizeof(event)) {
+ if (event_mode == STICK_MODE_EVENT ) {
+ /* Get events */
+ while (read(stick_device_handle,&event,sizeof(event))==sizeof(event)) {
- switch (event.type) {
- case EV_KEY:
- for (cnt = 0; cnt < stick_button_count; cnt++) {
- if (event.code == button_code[cnt]) {
- if (event.value) stick_button_values |= (1 << cnt); // Set bit
- else stick_button_values &= ~(1 << cnt); // Clear bit
- break;
+ switch (event.type) {
+ case EV_KEY:
+ for (cnt = 0; cnt < stick_button_count; cnt++) {
+ if (event.code == button_code[cnt]) {
+ if (event.value) stick_button_values |= (1 << cnt); // Set bit
+ else stick_button_values &= ~(1 << cnt); // Clear bit
+ break;
+ }
}
- }
- break;
- case EV_ABS:
- for (cnt = 0; cnt < stick_axis_count; cnt++) {
- if (event.code == axis_code[cnt]) {
- stick_axis_values[cnt] = (((event.value) - axis_min[cnt]))*ABS_MAX_VALUE / (axis_max[cnt] - axis_min[cnt]) - ABS_MID_VALUE;
- break;
+ break;
+ case EV_ABS:
+ for (cnt = 0; cnt < stick_axis_count; cnt++) {
+ if (event.code == axis_code[cnt]) {
+ stick_axis_values[cnt] = (((event.value) - axis_min[cnt]))*ABS_MAX_VALUE / (axis_max[cnt] - axis_min[cnt]) - ABS_MID_VALUE;
+ break;
+ }
}
- }
- break;
- default: break;
+ break;
+ default: break;
+ }
+ }
+
+ } else {
+
+ /* Get joystick events */
+ while (read(stick_device_handle,&jsevent,sizeof(jsevent))==sizeof(jsevent)) {
+
+ switch (jsevent.type) {
+ case JS_EVENT_BUTTON:
+ for (cnt = 0; cnt < stick_button_count; cnt++) {
+ if (jsevent.number == button_code[cnt]) {
+ if (jsevent.value) stick_button_values |= (1 << cnt); // Set bit
+ else stick_button_values &= ~(1 << cnt); // Clear bit
+ break;
+ }
+ }
+ break;
+ case JS_EVENT_AXIS:
+ for (cnt = 0; cnt < stick_axis_count; cnt++) {
+ if (jsevent.number == axis_code[cnt]) {
+ stick_axis_values[cnt] = (( (jsevent.value - axis_min[cnt]) * ABS_MAX_VALUE ) / (axis_max[cnt] - axis_min[cnt])) - ABS_MID_VALUE;
+ break;
+ }
+ }
+ break;
+ default: break;
+ }
}
}
- dbgprintf(stderr,"buttons %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d | ",
- (stick_button_values >> 0) & 1,
- (stick_button_values >> 1) & 1,
- (stick_button_values >> 2) & 1,
- (stick_button_values >> 3) & 1,
- (stick_button_values >> 4) & 1,
- (stick_button_values >> 5) & 1,
- (stick_button_values >> 6) & 1,
- (stick_button_values >> 7) & 1,
- (stick_button_values >> 8) & 1,
- (stick_button_values >> 9) & 1,
- (stick_button_values >> 10) & 1,
- (stick_button_values >> 11) & 1,
- (stick_button_values >> 12) & 1,
- (stick_button_values >> 13) & 1,
- (stick_button_values >> 14) & 1,
- (stick_button_values >> 15) & 1);
- dbgprintf(stderr,"axis %d %d %d %d %d %d %d %d %d %d\n",
- stick_axis_values[0],
- stick_axis_values[1],
- stick_axis_values[2],
- stick_axis_values[3],
- stick_axis_values[4],
- stick_axis_values[5],
- stick_axis_values[6],
- stick_axis_values[7],
- stick_axis_values[8],
- stick_axis_values[9]);
+ if (errno != EAGAIN) {
+ printf("Joystick read error!\n");
+ exit(1);
+ }
+
+ dbgprintf(stderr,"buttons ");
+ for (cnt = 0; cnt < stick_button_count; cnt++) {
+ dbgprintf(stderr,"%d ",(stick_button_values >> cnt) & 1 );
+ }
+
+ dbgprintf(stderr,"| axes ");
+ for (cnt = 0; cnt < stick_axis_count; cnt++) {
+ dbgprintf(stderr,"%d ",stick_axis_values[cnt]);
+ }
+
+ dbgprintf(stderr,"\n");
return 0;
}
@@ -318,7 +450,7 @@ int stick_read( void ) {
int stick_init( char * device_name ) {
- char devname[256];
+ char devname[MAX_NAME_LENGTH];
int cnt = 0;
/* test device_name, else look for a suitable device */
diff --git a/sw/ground_segment/joystick/usb_stick.h b/sw/ground_segment/joystick/usb_stick.h
index 65e709219e..babde27e4d 100644
--- a/sw/ground_segment/joystick/usb_stick.h
+++ b/sw/ground_segment/joystick/usb_stick.h
@@ -27,17 +27,26 @@
#include
/* Max number of axis and buttons */
-#define STICK_BUTTON_COUNT 16
-#define STICK_AXIS_COUNT 10
+/* Increased, many new controllers have pressure sensitive buttons that show up as axes */
+#define STICK_BUTTON_COUNT 32
+#define STICK_AXIS_COUNT 32
/* Default values for the options */
#define STICK_INPUT_DEV_MAX 15
#define STICK_DEVICE_NAME "/dev/input/event"
+/* Event mode switched */
+#define STICK_MODE_UNKNOWN 0
+#define STICK_MODE_EVENT 1
+#define STICK_MODE_JOYSTICK 2
+
+/* Max name length */
+#define MAX_NAME_LENGTH 255
+
/* Global variables about the initialized device */
extern int stick_device_handle;
extern int8_t stick_axis_values[STICK_AXIS_COUNT];
-extern int16_t stick_button_values;
+extern int32_t stick_button_values;
extern int stick_axis_count, stick_button_count;
/* Structure for custom configuration
diff --git a/sw/ground_segment/lpc21iap/lpc21iap.c b/sw/ground_segment/lpc21iap/lpc21iap.c
index 26dbd652a4..9cd73ff3be 100644
--- a/sw/ground_segment/lpc21iap/lpc21iap.c
+++ b/sw/ground_segment/lpc21iap/lpc21iap.c
@@ -24,7 +24,7 @@
#define LPC21IAP_VER_MAJ 1
-#define LPC21IAP_VER_MIN 1
+#define LPC21IAP_VER_MIN 2
#if defined(_WIN32) && !defined(__CYGWIN__)
@@ -156,9 +156,10 @@ int main(int argc, char *argv[])
tIntermediateBuffer* actBuf = NULL;
tIntermediateBuffer* startBuf = NULL;
+ printf("lpc21iap v%d.%d\n", LPC21IAP_VER_MAJ, LPC21IAP_VER_MIN);
+
if ((argc < 2) || (argc > 3))
{
- printf("lpc21iap version v%d.%d, ", LPC21IAP_VER_MAJ, LPC21IAP_VER_MIN);
printf("usage: %s file.elf [usb_serial_number]\n", argv[0]);
exit(1);
}
@@ -353,6 +354,12 @@ int main(int argc, char *argv[])
exit(2);
}
+ /* flashing code: persistent settings needs highest sector to be erased */
+ if (highFlash >= BOOTLOAD_SECT*MAX_SECT)
+ {
+ highFlash = maxFlash-1;
+ }
+
/* anything to flash erase? */
if (lowFlash != maxFlash)
{
diff --git a/sw/ground_segment/misc/Makefile b/sw/ground_segment/misc/Makefile
new file mode 100644
index 0000000000..018ab40905
--- /dev/null
+++ b/sw/ground_segment/misc/Makefile
@@ -0,0 +1,7 @@
+all: davis2ivy
+
+davis2ivy: davis2ivy.o
+ g++ -o davis2ivy davis2ivy.o -s -livy
+
+%.o : %.c
+ gcc -c -O2 -Wall $<
diff --git a/sw/ground_segment/misc/davis2ivy.c b/sw/ground_segment/misc/davis2ivy.c
new file mode 100644
index 0000000000..f167bb7815
--- /dev/null
+++ b/sw/ground_segment/misc/davis2ivy.c
@@ -0,0 +1,269 @@
+/*
+ * Paparazzi $Id$
+ *
+ * Copyright (C) 2011 Andreas Gaeb
+ *
+ * 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 davis2ivy.c
+ * \brief Connect a Davis VantagePro weather station to the Paparazzi system
+ *
+ * The program communicates with a Davis VantagePro(2) weather station connected
+ * to a serial port. It asks for new data (Davis' LOOP command) in the
+ * specified intervals, extracts the relevant data (ambient pressure and
+ * temperature, wind speed and direction) and broadcasts this via the Ivy bus.
+ *
+ * At the moment, the Ivy messages should be sent with the ID of the actually
+ * flying aircraft, which integrates them into the log file, as long as the
+ * aircraft sends its alive message.
+ *
+ * Useful links:
+ * - Weather Stations
+ * - Communication docs
+ *
+ */
+
+
+ #include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+
+#include
+#include
+#include
+
+
+typedef enum { FALSE = 0, TRUE } BOOL;
+
+#define PACKET_LENGTH 99
+
+
+// global variables
+int fd, ac_id = 1;
+const char *device;
+unsigned char packet[PACKET_LENGTH];
+TimerId tid;
+BOOL want_alive_msg = FALSE;
+
+
+/// Handler for Ctrl-C, exits the main loop
+void sigint_handler(int sig) {
+ IvyStop();
+ TimerRemove(tid);
+ close(fd);
+}
+
+/// open the serial port with the appropiate settings
+void open_port(const char* device) {
+ fd = open(device, O_RDWR | O_NOCTTY | O_NDELAY);
+ if (fd == -1) {
+ fprintf(stderr, "open_port: unable to open device %s - ", device);
+ perror(NULL);
+ exit(EXIT_FAILURE);
+ }
+ // setup connection options
+ struct termios options;
+
+ // get the current options
+ tcgetattr(fd, &options);
+
+ // set local mode, enable receiver, set comm. options:
+ // 8 data bits, 1 stop bit, no parity, 19200 Baud
+ options.c_cflag = CLOCAL | CREAD | CS8 | B19200;
+
+ // write options back to port
+ tcsetattr(fd, TCSANOW, &options);
+
+}
+
+/// disable transactions and empty queue
+void reset_station() {
+ char newline = '\n', bytes = 0;
+ fprintf(stderr, "Resetting communication\n");
+ // send a \n (wakeup and cancel all running transmits)
+ bytes = write(fd, &newline, 1);
+ // read and discard everything that might be left in the queue
+ close(fd);
+ sleep(1);
+ open_port(device);
+}
+
+/// send a wakeup call to the station
+BOOL wakeup(int tries) {
+ int loops = tries, bytes;
+ BOOL woken = FALSE;
+ char buf[] = {0, 0};
+ char newline = '\n';
+ do {
+ // send a \n
+ bytes = write(fd, &newline, 1);
+ // wait until station answers with \n\r
+ usleep(30000);
+ bytes = read(fd, buf, sizeof(buf));
+ woken = (buf[0] == 10) && (buf[1] == 13);
+ } while (!woken && loops-- > 0);
+ if (!woken) {
+ fprintf(stderr, "Could not wake up station: ");
+ if (bytes < 1) fprintf(stderr, "no bytes received\n");
+ else fprintf(stderr, "received %02x:%02x instead of \\n\\r\n", buf[0], buf[1]);
+ reset_station();
+ }
+ return woken;
+}
+
+/// send a LOOP command (read sensor data) to the station and get the packet back
+BOOL send_loop() {
+ char msg[32], ack;
+ // TODO maybe ask for more packets?
+ snprintf(msg, sizeof(msg), "LOOP %i\n", 1);
+ int bytes = write(fd, msg, strlen(msg));
+ usleep(120000);
+ bytes = read(fd, &ack, 1);
+ if (bytes < 1 || ack != 0x06) {
+ fprintf(stderr, "Failed to receive ACK from station\n");
+ reset_station();
+ return FALSE;
+ }
+ bytes = read(fd, packet, PACKET_LENGTH);
+ if (bytes < PACKET_LENGTH) {
+ fprintf(stderr, "Received packet is incomplete, only %i of %i bytes\n",
+ bytes, PACKET_LENGTH);
+ reset_station();
+ return FALSE;
+ } else {
+ return TRUE;
+ }
+}
+
+/// get the relevant data from the packet and sent it as Ivy message
+void decode_and_send_to_ivy() {
+
+ // check packet integrity
+ char expected[] = "LOO";
+ if (strncmp((char *)packet, expected, 3) != 0) {
+ fprintf(stderr, "Received packet from the weather station which does not match the expected format\n");
+ reset_station();
+ return;
+ }
+
+ // TODO CRC checking (is rather involved for the Davis protocol)
+
+ // get relevant data and convert to SI units
+ // see chapter IX.1 of the protocol definition
+ float
+ pstatic_Pa = (packet[7] | packet[8] << 8)*3.386388640341, // original is inches Hg / 1000
+ temp_degC = ((packet[12] | packet[13] << 8)/10.0 - 32.0)*5.0/9.0, // original is deg F / 10
+ windspeed_mps = packet[14]*0.44704, // original is miles per hour
+ winddir_deg = packet[16] | packet[17] << 8;
+
+
+ // TODO get the real MD5 for the aircraft id
+ if (want_alive_msg)
+ IvySendMsg("%d ALIVE 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0\n", ac_id);
+
+ // format has to match declaration in conf/messages.xml
+ IvySendMsg("%d WEATHER %f %f %f %f\n",
+ ac_id, pstatic_Pa, temp_degC, windspeed_mps, winddir_deg);
+}
+
+/// Get data from the station and send it via Ivy
+/** This function is executed by the timer
+ */
+void handle_timer (TimerId id, void *data, unsigned long delta) {
+ if (wakeup(3) && send_loop()) decode_and_send_to_ivy();
+}
+
+void print_usage(int argc, char ** argv) {
+ fprintf(stderr, "Usage: %s [-a] [-b ] [-d ] [-i ] [-s ]\n",
+ argv[0]);
+};
+
+/// Main function
+int main(int argc, char **argv) {
+ // default values for options
+ const char
+ *defaultbus = "127.255.255.255:2010",
+ *bus = defaultbus,
+ *defaultdevice = "/dev/ttyUSB1";
+ device = defaultdevice;
+ long delay = 1000;
+
+ // parse options
+ char c;
+ while ((c = getopt (argc, argv, "hab:d:i:s:")) != EOF) {
+ switch (c) {
+ case 'h':
+ print_usage(argc, argv);
+ exit(EXIT_SUCCESS);
+ break;
+ case 'a':
+ want_alive_msg = TRUE;
+ break;
+ case 'b':
+ bus = optarg;
+ break;
+ case 'd':
+ device = optarg;
+ break;
+ case 'i':
+ ac_id = atoi(optarg);
+ break;
+ case 's':
+ delay = atoi(optarg)*1000;
+ break;
+ case '?':
+ if (optopt == 'a' || optopt == 'b' || optopt == 'd' || optopt == 's')
+ fprintf (stderr, "Option -%c requires an argument.\n", optopt);
+ else if (isprint (optopt))
+ fprintf (stderr, "Unknown option `-%c'.\n", optopt);
+ else
+ fprintf (stderr, "Unknown option character `\\x%x'.\n", optopt);
+ print_usage(argc, argv);
+ exit(EXIT_FAILURE);
+ default:
+ abort ();
+ }
+ }
+
+
+ // make Ctrl-C stop the main loop and clean up properly
+ signal(SIGINT, sigint_handler);
+
+ bzero (packet, PACKET_LENGTH);
+ open_port(device);
+
+ // setup Ivy communication
+ IvyInit("davis2ivy", "READY", 0, 0, 0, 0);
+ IvyStart(bus);
+
+ // create timer
+ tid = TimerRepeatAfter (0, delay, handle_timer, 0);
+
+ IvyMainLoop();
+
+ return 0;
+}
diff --git a/sw/ground_segment/misc/readme.txt b/sw/ground_segment/misc/readme.txt
new file mode 100644
index 0000000000..5221c38c9f
--- /dev/null
+++ b/sw/ground_segment/misc/readme.txt
@@ -0,0 +1,3 @@
+davis2ivy:
+ A wrapper to communicate with a Davis VantagePro/VantagePro2 weather
+ station and integrate weather data into the telemetry link.
diff --git a/sw/ground_segment/tmtc/Makefile b/sw/ground_segment/tmtc/Makefile
index 9eb2547cec..72824e30df 100644
--- a/sw/ground_segment/tmtc/Makefile
+++ b/sw/ground_segment/tmtc/Makefile
@@ -48,7 +48,7 @@ OCAMLNETCMA=$(shell ocamlfind query -r -a-format -predicates byte netstring)
INCLUDES= -I $(OCAMLLIB) -I ../multimon $(shell ocamlfind query -r -i-format lablgtk2) $(shell ocamlfind query -r -i-format xml-light) $(OCAMLNETINCLUDES)
LIBPPRZCMA=$(OCAMLLIB)/lib-pprz.cma
-SERVERCMO = server_globals.cmo aircraft.cmo wind.cmo airprox.cmo kml.cmo fw_server.ml booz_server.ml server.cmo
+SERVERCMO = server_globals.cmo aircraft.cmo wind.cmo airprox.cmo kml.cmo fw_server.ml rotorcraft_server.ml server.cmo
SERVERCMX = $(SERVERCMO:.cmo=.cmx)
$(VAR)/boa.conf :$(CONF)/boa.conf
@@ -112,10 +112,6 @@ diadec : diadec.cmo ../multimon/multimon.cma
$(Q)$(OCAMLC) -custom $(INCLUDES) -o $@ unix.cma str.cma xml-light.cma lablgtk.cma glibivy-ocaml.cma lib-pprz.cma multimon.cma $^
-booz_translator : booz_translator.cmo ../../lib/ocaml/lib-pprz.cma
- @echo OL $@
- $(Q)$(OCAMLC) -custom $(INCLUDES) -o $@ unix.cma str.cma xml-light.cma lablgtk.cma glibivy-ocaml.cma lib-pprz.cma booz_translator.cmo
-
150m : 150m.cmo
@echo OL $@
$(Q)$(OCAMLC) -custom $(INCLUDES) -o $@ unix.cma str.cma xml-light.cma lablgtk.cma glibivy-ocaml.cma lib-pprz.cma gtkInit.cmo $^
diff --git a/sw/ground_segment/tmtc/aircraft.ml b/sw/ground_segment/tmtc/aircraft.ml
index dbc3118ca9..2d1c320df1 100644
--- a/sw/ground_segment/tmtc/aircraft.ml
+++ b/sw/ground_segment/tmtc/aircraft.ml
@@ -70,6 +70,11 @@ type nav_ref =
| Utm of Latlong.utm
| Ltp of Latlong.ecef
+type vehicle_type =
+ FixedWing
+ | Rotorcraft
+ | UnknownVehicleType
+
let add_pos_to_nav_ref = fun nav_ref ?(z = 0.) (x, y) ->
let rec lat_of_xy = fun lat last geo (_x, _y) n e ->
if n > 0 && abs_float (lat -. last) > e then
@@ -92,6 +97,7 @@ let add_pos_to_nav_ref = fun nav_ref ?(z = 0.) (x, y) ->
type waypoint = { altitude : float; wp_geo : Latlong.geographic }
type aircraft = {
+ mutable vehicle_type : vehicle_type;
id : string;
name : string;
flight_plan : Xml.xml;
@@ -150,7 +156,7 @@ let max_nb_dl_setting_values = 256 (** indexed iwth an uint8 (messages.xml) *)
let new_aircraft = fun id name fp airframe ->
let svsinfo_init = Array.init gps_nb_channels (fun _ -> svinfo_init ()) in
- { id = id ; name = name; flight_plan = fp; airframe = airframe;
+ { vehicle_type = UnknownVehicleType; id = id; name = name; flight_plan = fp; airframe = airframe;
pos = { Latlong.posn_lat = 0.; posn_long = 0. };
unix_time = 0.; itow = Int32.of_int 0;
roll = 0.; pitch = 0.;
diff --git a/sw/ground_segment/tmtc/aircraft.mli b/sw/ground_segment/tmtc/aircraft.mli
index f043e7f0c5..c4df220c03 100644
--- a/sw/ground_segment/tmtc/aircraft.mli
+++ b/sw/ground_segment/tmtc/aircraft.mli
@@ -54,11 +54,17 @@ type nav_ref =
| Utm of Latlong.utm
| Ltp of Latlong.ecef
+type vehicle_type =
+ FixedWing
+ | Rotorcraft
+ | UnknownVehicleType
+
val add_pos_to_nav_ref : nav_ref -> ?z:float -> (float * float) -> Latlong.geographic
type waypoint = { altitude : float; wp_geo : Latlong.geographic }
type aircraft = {
+ mutable vehicle_type : vehicle_type;
id : string;
name : string;
flight_plan : Xml.xml;
diff --git a/sw/ground_segment/tmtc/fw_server.ml b/sw/ground_segment/tmtc/fw_server.ml
index ce4a4fe2ba..4c1a4f7467 100644
--- a/sw/ground_segment/tmtc/fw_server.ml
+++ b/sw/ground_segment/tmtc/fw_server.ml
@@ -187,7 +187,8 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
a.bat <- fvalue "vsupply" /. 10.;
a.fbw.rc_rate <- ivalue "frame_rate"
| "PPRZ_MODE" ->
- a.ap_mode <- check_index (ivalue "ap_mode") ap_modes "AP_MODE";
+ a.vehicle_type <- FixedWing;
+ a.ap_mode <- check_index (ivalue "ap_mode") fixedwing_ap_modes "AP_MODE";
a.gaz_mode <- check_index (ivalue "ap_gaz") gaz_modes "AP_GAZ";
a.lateral_mode <- check_index (ivalue "ap_lateral") lat_modes "AP_LAT";
a.horizontal_mode <- check_index (ivalue "ap_horizontal") horiz_modes "AP_HORIZ";
diff --git a/sw/ground_segment/tmtc/kml.ml b/sw/ground_segment/tmtc/kml.ml
index e3b36ac49b..8b0fc9aeef 100644
--- a/sw/ground_segment/tmtc/kml.ml
+++ b/sw/ground_segment/tmtc/kml.ml
@@ -259,10 +259,15 @@ let update_ac = fun ac ->
else
sprintf "http://%s:%d/var/%s/flight_plan.kml" !hostname !port ac.name
in
+ let ap_mode = match ac.vehicle_type with
+ Rotorcraft -> rotorcraft_ap_modes.(ac.ap_mode)
+ | FixedWing -> fixedwing_ap_modes.(ac.ap_mode)
+ | UnknownVehicleType -> "UNK"
+ in
let blocks = ExtXml.child ac.flight_plan "blocks" in
let block = ExtXml.child blocks (string_of_int ac.cur_block) in
let block_name = ExtXml.attrib block "name" in
- let description = sprintf "%s : %s\nBat: %.1fV \nAGL: %.0fm\nSpeed: %.1fm/s" ap_modes.(ac.ap_mode) block_name ac.bat ac.agl ac.gspeed in
+ let description = sprintf "%s : %s\nBat: %.1fV \nAGL: %.0fm\nSpeed: %.1fm/s" ap_mode block_name ac.bat ac.agl ac.gspeed in
let change = change_placemark ~description ac.name ac.pos ac.alt in
let kml_changes = link_update url_flight_plan [change] in
print_xml ac.name "ac_changes.kml" kml_changes
diff --git a/sw/ground_segment/tmtc/booz_server.ml b/sw/ground_segment/tmtc/rotorcraft_server.ml
similarity index 95%
rename from sw/ground_segment/tmtc/booz_server.ml
rename to sw/ground_segment/tmtc/rotorcraft_server.ml
index a076cb9d27..7217143aac 100644
--- a/sw/ground_segment/tmtc/booz_server.ml
+++ b/sw/ground_segment/tmtc/rotorcraft_server.ml
@@ -1,7 +1,5 @@
(*
- * $Id: fw_server.ml,v 1.1 2009/03/22 17:53:48 hecto Exp $
- *
- * Server part specific to booz vehicles
+ * Server part specific to rotorcraft vehicles
*
* Copyright (C) ENAC
*
@@ -97,12 +95,13 @@ let update_waypoint = fun ac wp_id p alt ->
Not_found ->
Hashtbl.add ac.waypoints wp_id new_wp
-let get_pprz_mode = fun ap_mode ->
+(*let get_pprz_mode = fun ap_mode ->
let mode = ref 0 in
if ap_mode = 0 || ap_mode = 1 || ap_mode = 2 || ap_mode = 4 || ap_mode = 7 then mode := 0 (* MANUAL *)
else if ap_mode = 3 || ap_mode = 5 || ap_mode = 6 || ap_mode = 8 then mode := 1 (* AUTO1 *)
else if ap_mode = 9 || ap_mode = 10 || ap_mode = 11 || ap_mode = 12 then mode := 2; (* AUTO2 *)
!mode
+*)
let get_rc_status = fun rc_status ->
let status = ref "" in
@@ -178,13 +177,14 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
(*a.unix_time <- LL.unix_time_of_tow (truncate (fvalue "itow" /. 1000.));
a.itow <- Int32.of_float (fvalue "itow");*)
a.flight_time <- ivalue "flight_time";
- if a.gspeed > 3. && a.ap_mode = _AUTO2 then
- Wind.update ac_name a.gspeed a.course
+ (*if a.gspeed > 3. && a.ap_mode = _AUTO2 then
+ Wind.update ac_name a.gspeed a.course*)
| "ROTORCRAFT_STATUS" ->
+ a.vehicle_type <- Rotorcraft;
a.fbw.rc_status <- get_rc_status (ivalue "rc_status");
a.fbw.rc_rate <- ivalue "frame_rate";
a.gps_mode <- check_index (ivalue "gps_status") gps_modes "GPS_MODE";
- a.ap_mode <- check_index (get_pprz_mode (ivalue "ap_mode")) ap_modes "BOOZ_AP_MODE";
+ a.ap_mode <- check_index (ivalue "ap_mode") rotorcraft_ap_modes "ROTORCRAFT_AP_MODE";
a.kill_mode <- ivalue "ap_motors_on" == 0;
a.bat <- fvalue "vsupply" /. 10.;
| "INS_REF" ->
diff --git a/sw/ground_segment/tmtc/booz_server.mli b/sw/ground_segment/tmtc/rotorcraft_server.mli
similarity index 89%
rename from sw/ground_segment/tmtc/booz_server.mli
rename to sw/ground_segment/tmtc/rotorcraft_server.mli
index 14fc8f6fc8..9cbf3fdc0f 100644
--- a/sw/ground_segment/tmtc/booz_server.mli
+++ b/sw/ground_segment/tmtc/rotorcraft_server.mli
@@ -1,7 +1,5 @@
(*
- * $Id: fw_server.mli,v 1.1 2009/03/22 17:53:48 hecto Exp $
- *
- * Server part specific to booz vehicles
+ * Server part specific to rotorcraft vehicles
*
* Copyright (C) ENAC
*
diff --git a/sw/ground_segment/tmtc/server.ml b/sw/ground_segment/tmtc/server.ml
index cdc31f6daa..747b63dccb 100644
--- a/sw/ground_segment/tmtc/server.ml
+++ b/sw/ground_segment/tmtc/server.ml
@@ -52,6 +52,12 @@ let srtm_path = Env.paparazzi_home // "data" // "srtm"
let get_indexed_value = fun t i ->
if i >= 0 then t.(i) else "UNK"
+let modes_of_type = fun vt ->
+ match vt with
+ FixedWing -> fixedwing_ap_modes
+ | Rotorcraft -> rotorcraft_ap_modes
+ | UnknownVehicleType -> [| |]
+
(** The aircrafts store *)
let aircrafts = Hashtbl.create 3
@@ -135,7 +141,7 @@ let ac_msg = fun messages_xml logging ac_name ac ->
let msg = Tele_Pprz.message_of_id msg_id in
log ?timestamp logging ac_name msg.Pprz.name values;
Fw_server.log_and_parse ac_name ac msg values;
- Booz_server.log_and_parse ac_name ac msg values
+ Rotorcraft_server.log_and_parse ac_name ac msg values
with
Telemetry_error (ac_name, msg) ->
Ground_Pprz.message_send my_id "TELEMETRY_ERROR" ["ac_id", Pprz.String ac_name;"message", Pprz.String msg];
@@ -355,7 +361,7 @@ let send_aircraft_msg = fun ac ->
"energy", Pprz.Int a.energy] in
Ground_Pprz.message_send my_id "ENGINE_STATUS" values;
- let ap_mode = get_indexed_value ap_modes a.ap_mode in
+ let ap_mode = get_indexed_value (modes_of_type a.vehicle_type) a.ap_mode in
let gaz_mode = get_indexed_value gaz_modes a.gaz_mode in
let lat_mode = get_indexed_value lat_modes a.lateral_mode in
let horiz_mode = get_indexed_value horiz_modes a.horizontal_mode in
diff --git a/sw/ground_segment/tmtc/server_globals.ml b/sw/ground_segment/tmtc/server_globals.ml
index 0255bca0b1..029a66bb68 100644
--- a/sw/ground_segment/tmtc/server_globals.ml
+++ b/sw/ground_segment/tmtc/server_globals.ml
@@ -3,7 +3,8 @@ exception Telemetry_error of string * string
let hostname = ref "localhost"
(** FIXME: Should be read from messages.xml *)
-let ap_modes = [|"MANUAL";"AUTO1";"AUTO2";"HOME";"NOGPS"|]
+let fixedwing_ap_modes = [|"MANUAL";"AUTO1";"AUTO2";"HOME";"NOGPS"|]
+let rotorcraft_ap_modes = [|"SAFE";"KILL";"RATE";"ATT";"R_RCC";"A_RCC";"ATT_C";"R_ZH";"A_ZH";"HOVER";"HOV_C";"H_ZH";"NAV"|]
let _AUTO2 = 2
let gaz_modes = [|"MANUAL";"GAZ";"CLIMB";"ALT"|]
let lat_modes = [|"MANUAL";"ROLL_RATE";"ROLL";"COURSE"|]
diff --git a/sw/in_progress/pow/InstallCert.java b/sw/in_progress/pow/InstallCert.java
deleted file mode 100644
index f979cc7868..0000000000
--- a/sw/in_progress/pow/InstallCert.java
+++ /dev/null
@@ -1,188 +0,0 @@
-
-/*
- * Copyright 2006 Sun Microsystems, Inc. All Rights Reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * - Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- *
- * - Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in the
- * documentation and/or other materials provided with the distribution.
- *
- * - Neither the name of Sun Microsystems nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
- * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
- * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
- * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
- * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
- * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
- * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
- * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
- * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
- * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-/**
- * @see http://blogs.sun.com/gc/entry/unable_to_find_valid_certification
- */
-import java.io.*;
-
-import java.security.*;
-import java.security.cert.*;
-
-import javax.net.ssl.*;
-
-public class InstallCert {
-
- public static void main(String[] args) throws Exception {
- String host;
- int port;
- char[] passphrase;
- if ((args.length == 1) || (args.length == 2)) {
- String[] c = args[0].split(":");
- host = c[0];
- port = (c.length == 1) ? 443 : Integer.parseInt(c[1]);
- String p = (args.length == 1) ? "changeit" : args[1];
- passphrase = p.toCharArray();
- } else {
- System.out.println("Usage: java InstallCert [:port] [passphrase]");
- return;
- }
-
- File file = new File("jssecacerts");
- if (file.isFile() == false) {
- char SEP = File.separatorChar;
- File dir = new File(System.getProperty("java.home") + SEP
- + "lib" + SEP + "security");
- file = new File(dir, "jssecacerts");
- if (file.isFile() == false) {
- file = new File(dir, "cacerts");
- }
- }
- System.out.println("Loading KeyStore " + file + "...");
- InputStream in = new FileInputStream(file);
- KeyStore ks = KeyStore.getInstance(KeyStore.getDefaultType());
- ks.load(in, passphrase);
- in.close();
-
- SSLContext context = SSLContext.getInstance("TLS");
- TrustManagerFactory tmf =
- TrustManagerFactory.getInstance(TrustManagerFactory.getDefaultAlgorithm());
- tmf.init(ks);
- X509TrustManager defaultTrustManager = (X509TrustManager)tmf.getTrustManagers()[0];
- SavingTrustManager tm = new SavingTrustManager(defaultTrustManager);
- context.init(null, new TrustManager[] {tm}, null);
- SSLSocketFactory factory = context.getSocketFactory();
-
- System.out.println("Opening connection to " + host + ":" + port + "...");
- SSLSocket socket = (SSLSocket)factory.createSocket(host, port);
- socket.setSoTimeout(10000);
- try {
- System.out.println("Starting SSL handshake...");
- socket.startHandshake();
- socket.close();
- System.out.println();
- System.out.println("No errors, certificate is already trusted");
- } catch (SSLException e) {
- System.out.println();
- e.printStackTrace(System.out);
- }
-
- X509Certificate[] chain = tm.chain;
- if (chain == null) {
- System.out.println("Could not obtain server certificate chain");
- return;
- }
-
- BufferedReader reader =
- new BufferedReader(new InputStreamReader(System.in));
-
- System.out.println();
- System.out.println("Server sent " + chain.length + " certificate(s):");
- System.out.println();
- MessageDigest sha1 = MessageDigest.getInstance("SHA1");
- MessageDigest md5 = MessageDigest.getInstance("MD5");
- for (int i = 0; i < chain.length; i++) {
- X509Certificate cert = chain[i];
- System.out.println
- (" " + (i + 1) + " Subject " + cert.getSubjectDN());
- System.out.println(" Issuer " + cert.getIssuerDN());
- sha1.update(cert.getEncoded());
- System.out.println(" sha1 " + toHexString(sha1.digest()));
- md5.update(cert.getEncoded());
- System.out.println(" md5 " + toHexString(md5.digest()));
- System.out.println();
- }
-
- System.out.println("Enter certificate to add to trusted keystore or 'q' to quit: [1]");
- String line = reader.readLine().trim();
- int k;
- try {
- k = (line.length() == 0) ? 0 : Integer.parseInt(line) - 1;
- } catch (NumberFormatException e) {
- System.out.println("KeyStore not changed");
- return;
- }
-
- X509Certificate cert = chain[k];
- String alias = host + "-" + (k + 1);
- ks.setCertificateEntry(alias, cert);
-
- OutputStream out = new FileOutputStream("jssecacerts");
- ks.store(out, passphrase);
- out.close();
-
- System.out.println();
- System.out.println(cert);
- System.out.println();
- System.out.println
- ("Added certificate to keystore 'jssecacerts' using alias '"
- + alias + "'");
- }
-
- private static final char[] HEXDIGITS = "0123456789abcdef".toCharArray();
-
- private static String toHexString(byte[] bytes) {
- StringBuilder sb = new StringBuilder(bytes.length * 3);
- for (int b : bytes) {
- b &= 0xff;
- sb.append(HEXDIGITS[b >> 4]);
- sb.append(HEXDIGITS[b & 15]);
- sb.append(' ');
- }
- return sb.toString();
- }
-
- private static class SavingTrustManager implements X509TrustManager {
-
- private final X509TrustManager tm;
- private X509Certificate[] chain;
-
- SavingTrustManager(X509TrustManager tm) {
- this.tm = tm;
- }
-
- public X509Certificate[] getAcceptedIssuers() {
- throw new UnsupportedOperationException();
- }
-
- public void checkClientTrusted(X509Certificate[] chain, String authType)
- throws CertificateException {
- throw new UnsupportedOperationException();
- }
-
- public void checkServerTrusted(X509Certificate[] chain, String authType)
- throws CertificateException {
- this.chain = chain;
- tm.checkServerTrusted(chain, authType);
- }
- }
-
-}
diff --git a/sw/in_progress/pow/ServletPow/InstallCert.java b/sw/in_progress/pow/ServletPow/InstallCert.java
deleted file mode 100644
index f979cc7868..0000000000
--- a/sw/in_progress/pow/ServletPow/InstallCert.java
+++ /dev/null
@@ -1,188 +0,0 @@
-
-/*
- * Copyright 2006 Sun Microsystems, Inc. All Rights Reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * - Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- *
- * - Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in the
- * documentation and/or other materials provided with the distribution.
- *
- * - Neither the name of Sun Microsystems nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
- * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
- * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
- * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
- * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
- * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
- * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
- * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
- * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
- * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-/**
- * @see http://blogs.sun.com/gc/entry/unable_to_find_valid_certification
- */
-import java.io.*;
-
-import java.security.*;
-import java.security.cert.*;
-
-import javax.net.ssl.*;
-
-public class InstallCert {
-
- public static void main(String[] args) throws Exception {
- String host;
- int port;
- char[] passphrase;
- if ((args.length == 1) || (args.length == 2)) {
- String[] c = args[0].split(":");
- host = c[0];
- port = (c.length == 1) ? 443 : Integer.parseInt(c[1]);
- String p = (args.length == 1) ? "changeit" : args[1];
- passphrase = p.toCharArray();
- } else {
- System.out.println("Usage: java InstallCert [:port] [passphrase]");
- return;
- }
-
- File file = new File("jssecacerts");
- if (file.isFile() == false) {
- char SEP = File.separatorChar;
- File dir = new File(System.getProperty("java.home") + SEP
- + "lib" + SEP + "security");
- file = new File(dir, "jssecacerts");
- if (file.isFile() == false) {
- file = new File(dir, "cacerts");
- }
- }
- System.out.println("Loading KeyStore " + file + "...");
- InputStream in = new FileInputStream(file);
- KeyStore ks = KeyStore.getInstance(KeyStore.getDefaultType());
- ks.load(in, passphrase);
- in.close();
-
- SSLContext context = SSLContext.getInstance("TLS");
- TrustManagerFactory tmf =
- TrustManagerFactory.getInstance(TrustManagerFactory.getDefaultAlgorithm());
- tmf.init(ks);
- X509TrustManager defaultTrustManager = (X509TrustManager)tmf.getTrustManagers()[0];
- SavingTrustManager tm = new SavingTrustManager(defaultTrustManager);
- context.init(null, new TrustManager[] {tm}, null);
- SSLSocketFactory factory = context.getSocketFactory();
-
- System.out.println("Opening connection to " + host + ":" + port + "...");
- SSLSocket socket = (SSLSocket)factory.createSocket(host, port);
- socket.setSoTimeout(10000);
- try {
- System.out.println("Starting SSL handshake...");
- socket.startHandshake();
- socket.close();
- System.out.println();
- System.out.println("No errors, certificate is already trusted");
- } catch (SSLException e) {
- System.out.println();
- e.printStackTrace(System.out);
- }
-
- X509Certificate[] chain = tm.chain;
- if (chain == null) {
- System.out.println("Could not obtain server certificate chain");
- return;
- }
-
- BufferedReader reader =
- new BufferedReader(new InputStreamReader(System.in));
-
- System.out.println();
- System.out.println("Server sent " + chain.length + " certificate(s):");
- System.out.println();
- MessageDigest sha1 = MessageDigest.getInstance("SHA1");
- MessageDigest md5 = MessageDigest.getInstance("MD5");
- for (int i = 0; i < chain.length; i++) {
- X509Certificate cert = chain[i];
- System.out.println
- (" " + (i + 1) + " Subject " + cert.getSubjectDN());
- System.out.println(" Issuer " + cert.getIssuerDN());
- sha1.update(cert.getEncoded());
- System.out.println(" sha1 " + toHexString(sha1.digest()));
- md5.update(cert.getEncoded());
- System.out.println(" md5 " + toHexString(md5.digest()));
- System.out.println();
- }
-
- System.out.println("Enter certificate to add to trusted keystore or 'q' to quit: [1]");
- String line = reader.readLine().trim();
- int k;
- try {
- k = (line.length() == 0) ? 0 : Integer.parseInt(line) - 1;
- } catch (NumberFormatException e) {
- System.out.println("KeyStore not changed");
- return;
- }
-
- X509Certificate cert = chain[k];
- String alias = host + "-" + (k + 1);
- ks.setCertificateEntry(alias, cert);
-
- OutputStream out = new FileOutputStream("jssecacerts");
- ks.store(out, passphrase);
- out.close();
-
- System.out.println();
- System.out.println(cert);
- System.out.println();
- System.out.println
- ("Added certificate to keystore 'jssecacerts' using alias '"
- + alias + "'");
- }
-
- private static final char[] HEXDIGITS = "0123456789abcdef".toCharArray();
-
- private static String toHexString(byte[] bytes) {
- StringBuilder sb = new StringBuilder(bytes.length * 3);
- for (int b : bytes) {
- b &= 0xff;
- sb.append(HEXDIGITS[b >> 4]);
- sb.append(HEXDIGITS[b & 15]);
- sb.append(' ');
- }
- return sb.toString();
- }
-
- private static class SavingTrustManager implements X509TrustManager {
-
- private final X509TrustManager tm;
- private X509Certificate[] chain;
-
- SavingTrustManager(X509TrustManager tm) {
- this.tm = tm;
- }
-
- public X509Certificate[] getAcceptedIssuers() {
- throw new UnsupportedOperationException();
- }
-
- public void checkClientTrusted(X509Certificate[] chain, String authType)
- throws CertificateException {
- throw new UnsupportedOperationException();
- }
-
- public void checkServerTrusted(X509Certificate[] chain, String authType)
- throws CertificateException {
- this.chain = chain;
- tm.checkServerTrusted(chain, authType);
- }
- }
-
-}
diff --git a/sw/in_progress/pow/ServletPow/JARLIB/commons-codec-1.4.jar b/sw/in_progress/pow/ServletPow/JARLIB/commons-codec-1.4.jar
deleted file mode 100755
index 458d432da8..0000000000
Binary files a/sw/in_progress/pow/ServletPow/JARLIB/commons-codec-1.4.jar and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/JARLIB/commons-fileupload-1.2.1.jar b/sw/in_progress/pow/ServletPow/JARLIB/commons-fileupload-1.2.1.jar
deleted file mode 100755
index aa209b3887..0000000000
Binary files a/sw/in_progress/pow/ServletPow/JARLIB/commons-fileupload-1.2.1.jar and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/JARLIB/commons-httpclient-3.1.jar b/sw/in_progress/pow/ServletPow/JARLIB/commons-httpclient-3.1.jar
deleted file mode 100755
index 7c59774aed..0000000000
Binary files a/sw/in_progress/pow/ServletPow/JARLIB/commons-httpclient-3.1.jar and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/JARLIB/commons-io-1.4.jar b/sw/in_progress/pow/ServletPow/JARLIB/commons-io-1.4.jar
deleted file mode 100755
index 133dc6cb35..0000000000
Binary files a/sw/in_progress/pow/ServletPow/JARLIB/commons-io-1.4.jar and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/JARLIB/commons-logging-1.1.1.jar b/sw/in_progress/pow/ServletPow/JARLIB/commons-logging-1.1.1.jar
deleted file mode 100755
index 8758a96b70..0000000000
Binary files a/sw/in_progress/pow/ServletPow/JARLIB/commons-logging-1.1.1.jar and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/JARLIB/ivy-1.2.13.jar b/sw/in_progress/pow/ServletPow/JARLIB/ivy-1.2.13.jar
deleted file mode 100755
index be51c0e1f0..0000000000
Binary files a/sw/in_progress/pow/ServletPow/JARLIB/ivy-1.2.13.jar and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/JARLIB/jakarta-regexp-1.5.jar b/sw/in_progress/pow/ServletPow/JARLIB/jakarta-regexp-1.5.jar
deleted file mode 100755
index 652bc822c9..0000000000
Binary files a/sw/in_progress/pow/ServletPow/JARLIB/jakarta-regexp-1.5.jar and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/JARLIB/jdom.jar b/sw/in_progress/pow/ServletPow/JARLIB/jdom.jar
deleted file mode 100755
index 65a1b3f737..0000000000
Binary files a/sw/in_progress/pow/ServletPow/JARLIB/jdom.jar and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/JARLIB/log4j-1.2.16.jar b/sw/in_progress/pow/ServletPow/JARLIB/log4j-1.2.16.jar
deleted file mode 100755
index 3f9d847618..0000000000
Binary files a/sw/in_progress/pow/ServletPow/JARLIB/log4j-1.2.16.jar and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/JARLIB/mysql-connector-java-5.1.12-bin.jar b/sw/in_progress/pow/ServletPow/JARLIB/mysql-connector-java-5.1.12-bin.jar
deleted file mode 100755
index af5847eed4..0000000000
Binary files a/sw/in_progress/pow/ServletPow/JARLIB/mysql-connector-java-5.1.12-bin.jar and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/JARLIB/pushlet.jar b/sw/in_progress/pow/ServletPow/JARLIB/pushlet.jar
deleted file mode 100755
index 6dc1c22e21..0000000000
Binary files a/sw/in_progress/pow/ServletPow/JARLIB/pushlet.jar and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/JARLIB/pushletclient.jar b/sw/in_progress/pow/ServletPow/JARLIB/pushletclient.jar
deleted file mode 100755
index c699b8b25f..0000000000
Binary files a/sw/in_progress/pow/ServletPow/JARLIB/pushletclient.jar and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/JARLIB/rome-1.0.jar b/sw/in_progress/pow/ServletPow/JARLIB/rome-1.0.jar
deleted file mode 100755
index 7138baaca1..0000000000
Binary files a/sw/in_progress/pow/ServletPow/JARLIB/rome-1.0.jar and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/JARLIB/serializer.jar b/sw/in_progress/pow/ServletPow/JARLIB/serializer.jar
deleted file mode 100755
index de9b007b4c..0000000000
Binary files a/sw/in_progress/pow/ServletPow/JARLIB/serializer.jar and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/JARLIB/servlet-api.jar b/sw/in_progress/pow/ServletPow/JARLIB/servlet-api.jar
deleted file mode 100755
index a71c371333..0000000000
Binary files a/sw/in_progress/pow/ServletPow/JARLIB/servlet-api.jar and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/JARLIB/xerces.jar b/sw/in_progress/pow/ServletPow/JARLIB/xerces.jar
deleted file mode 100755
index e9fe08ad45..0000000000
Binary files a/sw/in_progress/pow/ServletPow/JARLIB/xerces.jar and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/ServletPow.war b/sw/in_progress/pow/ServletPow/ServletPow.war
deleted file mode 100755
index 0888047d40..0000000000
Binary files a/sw/in_progress/pow/ServletPow/ServletPow.war and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/conf apache2/mods_available/jk.load b/sw/in_progress/pow/ServletPow/conf apache2/mods_available/jk.load
deleted file mode 100755
index 841cf3d0c2..0000000000
--- a/sw/in_progress/pow/ServletPow/conf apache2/mods_available/jk.load
+++ /dev/null
@@ -1,6 +0,0 @@
-LoadModule jk_module /usr/lib/apache2/modules/mod_jk.so
-
-JkWorkersFile /etc/apache2/workers.properties
-JkLogFile /var/log/apache2/mod_jk.log
-JkLogLevel debug
-JkLogStampFormat "[%a %b %d %H:%M:%S %Y] "
diff --git a/sw/in_progress/pow/ServletPow/conf apache2/mods_available/php5.conf b/sw/in_progress/pow/ServletPow/conf apache2/mods_available/php5.conf
deleted file mode 100755
index 9152fe14f6..0000000000
--- a/sw/in_progress/pow/ServletPow/conf apache2/mods_available/php5.conf
+++ /dev/null
@@ -1,20 +0,0 @@
-
-AddType application/x-httpd-php .php .phtml .php3
-AddType application/x-httpd-php-source .phps
-
-
-
- SetHandler application/x-httpd-php
-
-
- SetHandler application/x-httpd-php-source
-
- # To re-enable php in user directories comment the following lines
- # (from to .) Do NOT set it to On as it
- # prevents .htaccess files from disabling it.
-
-
- php_admin_value engine Off
-
-
-
diff --git a/sw/in_progress/pow/ServletPow/conf apache2/mods_available/php5.load b/sw/in_progress/pow/ServletPow/conf apache2/mods_available/php5.load
deleted file mode 100755
index 0d6a55eb43..0000000000
--- a/sw/in_progress/pow/ServletPow/conf apache2/mods_available/php5.load
+++ /dev/null
@@ -1 +0,0 @@
-LoadModule php5_module /usr/lib/apache2/modules/libphp5.so
\ No newline at end of file
diff --git a/sw/in_progress/pow/ServletPow/conf apache2/mods_available/ssl.conf b/sw/in_progress/pow/ServletPow/conf apache2/mods_available/ssl.conf
deleted file mode 100755
index 1e4ce40c44..0000000000
--- a/sw/in_progress/pow/ServletPow/conf apache2/mods_available/ssl.conf
+++ /dev/null
@@ -1,64 +0,0 @@
-
-#
-# Pseudo Random Number Generator (PRNG):
-# Configure one or more sources to seed the PRNG of the SSL library.
-# The seed data should be of good random quality.
-# WARNING! On some platforms /dev/random blocks if not enough entropy
-# is available. This means you then cannot use the /dev/random device
-# because it would lead to very long connection times (as long as
-# it requires to make more entropy available). But usually those
-# platforms additionally provide a /dev/urandom device which doesn't
-# block. So, if available, use this one instead. Read the mod_ssl User
-# Manual for more details.
-#
-SSLRandomSeed startup builtin
-SSLRandomSeed startup file:/dev/urandom 512
-SSLRandomSeed connect builtin
-SSLRandomSeed connect file:/dev/urandom 512
-
-##
-## SSL Global Context
-##
-## All SSL configuration in this context applies both to
-## the main server and all SSL-enabled virtual hosts.
-##
-
-#
-# Some MIME-types for downloading Certificates and CRLs
-#
-AddType application/x-x509-ca-cert .crt
-AddType application/x-pkcs7-crl .crl
-
-# Pass Phrase Dialog:
-# Configure the pass phrase gathering process.
-# The filtering dialog program (`builtin' is a internal
-# terminal dialog) has to provide the pass phrase on stdout.
-SSLPassPhraseDialog builtin
-
-# Inter-Process Session Cache:
-# Configure the SSL Session Cache: First the mechanism
-# to use and second the expiring timeout (in seconds).
-#SSLSessionCache dbm:/var/run/apache2/ssl_scache
-SSLSessionCache shmcb:/var/run/apache2/ssl_scache(512000)
-SSLSessionCacheTimeout 300
-
-# Semaphore:
-# Configure the path to the mutual exclusion semaphore the
-# SSL engine uses internally for inter-process synchronization.
-SSLMutex file:/var/run/apache2/ssl_mutex
-
-# SSL Cipher Suite:
-# List the ciphers that the client is permitted to negotiate.
-# See the mod_ssl documentation for a complete list.
-# enable only secure ciphers:
-SSLCipherSuite HIGH:MEDIUM:!ADH
-# Use this instead if you want to allow cipher upgrades via SGC facility.
-# In this case you also have to use something like
-# SSLRequire %{SSL_CIPHER_USEKEYSIZE} >= 128
-# see http://httpd.apache.org/docs/2.2/ssl/ssl_howto.html.en#upgradeenc
-#SSLCipherSuite ALL:!ADH:!EXPORT56:RC4+RSA:+HIGH:+MEDIUM:+LOW:+SSLv2:+EXP:+eNULL
-
-# enable only secure protocols: SSLv3 and TLSv1, but not SSLv2
-SSLProtocol all -SSLv2
-
-
diff --git a/sw/in_progress/pow/ServletPow/conf apache2/mods_available/ssl.load b/sw/in_progress/pow/ServletPow/conf apache2/mods_available/ssl.load
deleted file mode 100755
index ff861daab5..0000000000
--- a/sw/in_progress/pow/ServletPow/conf apache2/mods_available/ssl.load
+++ /dev/null
@@ -1 +0,0 @@
-LoadModule ssl_module /usr/lib/apache2/modules/mod_ssl.so
diff --git a/sw/in_progress/pow/ServletPow/conf apache2/ports.conf b/sw/in_progress/pow/ServletPow/conf apache2/ports.conf
deleted file mode 100755
index 7830895dc0..0000000000
--- a/sw/in_progress/pow/ServletPow/conf apache2/ports.conf
+++ /dev/null
@@ -1,23 +0,0 @@
-# If you just change the port or add more ports here, you will likely also
-# have to change the VirtualHost statement in
-# /etc/apache2/sites-enabled/000-default
-# This is also true if you have upgraded from before 2.2.9-3 (i.e. from
-# Debian etch). See /usr/share/doc/apache2.2-common/NEWS.Debian.gz and
-# README.Debian.gz
-
-#NameVirtualHost *:80
-Listen 80
-
-
- # If you add NameVirtualHost *:443 here, you will also have to change
- # the VirtualHost statement in /etc/apache2/sites-available/default-ssl
- # to
- # Server Name Indication for SSL named virtual hosts is currently not
- # supported by MSIE on Windows XP.
- Listen 443
-
-
-
- Listen 443
-
-
diff --git a/sw/in_progress/pow/ServletPow/conf apache2/sites-available/default b/sw/in_progress/pow/ServletPow/conf apache2/sites-available/default
deleted file mode 100755
index 0559ce148e..0000000000
--- a/sw/in_progress/pow/ServletPow/conf apache2/sites-available/default
+++ /dev/null
@@ -1,42 +0,0 @@
-NameVirtualHost *:80
-
- ServerAdmin webmaster@localhost
-
- DocumentRoot /var/www
-
- Options FollowSymLinks
- AllowOverride None
-
-
- Options Indexes FollowSymLinks MultiViews
- AllowOverride None
- Order allow,deny
- allow from all
-
-
- ScriptAlias /cgi-bin/ /usr/lib/cgi-bin/
-
- AllowOverride None
- Options +ExecCGI -MultiViews +SymLinksIfOwnerMatch
- Order allow,deny
- Allow from all
-
-
- ErrorLog /var/log/apache2/error.log
-
- # Possible values include: debug, info, notice, warn, error, crit,
- # alert, emerg.
- LogLevel warn
-
- CustomLog /var/log/apache2/access.log combined
-
- Alias /doc/ "/usr/share/doc/"
-
- Options Indexes MultiViews FollowSymLinks
- AllowOverride None
- Order deny,allow
- Deny from all
- Allow from 127.0.0.0/255.0.0.0 ::1/128
-
-
-
diff --git a/sw/in_progress/pow/ServletPow/conf apache2/sites-available/powV2_ssl b/sw/in_progress/pow/ServletPow/conf apache2/sites-available/powV2_ssl
deleted file mode 100755
index 7a4efcaca3..0000000000
--- a/sw/in_progress/pow/ServletPow/conf apache2/sites-available/powV2_ssl
+++ /dev/null
@@ -1,42 +0,0 @@
-
-NameVirtualHost *:443
-
-
- ServerAdmin admin@pow.fr
- ServerName blanc
-
- LogLevel warn
-
- CustomLog /var/log/apache2/access.log combined
- ServerSignature On
-
- SSLEngine on
- SSLCipherSuite ALL:!ADH:!EXPORT56:RC4+RSA:+HIGH:+MEDIUM:+LOW:+SSLv2:+EXP
- #SSLCertificateFile /etc/ssl/private/serveur.cert
- #SSLCertificateKeyFile /etc/ssl/private/serveur.key
- #SSLCACertificateFile /etc/ssl/private/ca-root.cert
- SSLCertificateFile /etc/ssl/server.crt
- SSLCertificateKeyFile /etc/ssl/server.key
- SSLCACertificatePath /etc/ssl
-
-#SSLVerifyClient optional_no_ca
-#SSLOptions +ExportCertData +StdEnvVars
- SetEnvIf User-Agent ".*MSIE.*" \
- nokeepalive ssl-unclean-shutdown \
- downgrade-1.0 force-response-1.0
-
- JkExtractSSL On
- JkOptions +FlushPackets
- JkOptions +FlushHeader
- JkMount /ServletPow worker1
- JkMount /ServletPow/* worker1
- JkMount /examples/* worker1
- JkMount /examples worker1
-
-#Redirect https://www.recherche.enac.fr/TestServletPow https://www.recherche.enac.fr/evaluation/TestServletPow
-
-
-
-
-
-
diff --git a/sw/in_progress/pow/ServletPow/conf apache2/workers.properties b/sw/in_progress/pow/ServletPow/conf apache2/workers.properties
deleted file mode 100755
index 996a10559b..0000000000
--- a/sw/in_progress/pow/ServletPow/conf apache2/workers.properties
+++ /dev/null
@@ -1,9 +0,0 @@
-workers.tomcat_home=/users/genin/apache-tomcat-6.0.26
-
-workers.java_home=/usr/lib/jvm/java-6-sun
-ps=/
-worker.list=worker1
-worker.worker1.port=8009
-worker.worker1.host=localhost
-worker.worker1.type=ajp13
-worker.worker1.lbfactor=1
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/.classpath b/sw/in_progress/pow/ServletPow/eclipse ServletPow/.classpath
deleted file mode 100755
index fd0f78ce42..0000000000
--- a/sw/in_progress/pow/ServletPow/eclipse ServletPow/.classpath
+++ /dev/null
@@ -1,26 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/.project b/sw/in_progress/pow/ServletPow/eclipse ServletPow/.project
deleted file mode 100755
index 3470d17f2e..0000000000
--- a/sw/in_progress/pow/ServletPow/eclipse ServletPow/.project
+++ /dev/null
@@ -1,36 +0,0 @@
-
-
- ServletPow
-
-
-
-
-
- org.eclipse.wst.jsdt.core.javascriptValidator
-
-
-
-
- org.eclipse.jdt.core.javabuilder
-
-
-
-
- org.eclipse.wst.common.project.facet.core.builder
-
-
-
-
- org.eclipse.wst.validation.validationbuilder
-
-
-
-
-
- org.eclipse.jem.workbench.JavaEMFNature
- org.eclipse.wst.common.modulecore.ModuleCoreNature
- org.eclipse.wst.common.project.facet.core.nature
- org.eclipse.jdt.core.javanature
- org.eclipse.wst.jsdt.core.jsNature
-
-
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/.settings/.jsdtscope b/sw/in_progress/pow/ServletPow/eclipse ServletPow/.settings/.jsdtscope
deleted file mode 100755
index bbb8e68be8..0000000000
--- a/sw/in_progress/pow/ServletPow/eclipse ServletPow/.settings/.jsdtscope
+++ /dev/null
@@ -1,11 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/.settings/org.eclipse.jdt.core.prefs b/sw/in_progress/pow/ServletPow/eclipse ServletPow/.settings/org.eclipse.jdt.core.prefs
deleted file mode 100755
index 420b6fb9b2..0000000000
--- a/sw/in_progress/pow/ServletPow/eclipse ServletPow/.settings/org.eclipse.jdt.core.prefs
+++ /dev/null
@@ -1,7 +0,0 @@
-#Tue Apr 20 15:09:25 CEST 2010
-eclipse.preferences.version=1
-org.eclipse.jdt.core.compiler.codegen.targetPlatform=1.6
-org.eclipse.jdt.core.compiler.compliance=1.6
-org.eclipse.jdt.core.compiler.problem.assertIdentifier=error
-org.eclipse.jdt.core.compiler.problem.enumIdentifier=error
-org.eclipse.jdt.core.compiler.source=1.6
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/.settings/org.eclipse.wst.common.component b/sw/in_progress/pow/ServletPow/eclipse ServletPow/.settings/org.eclipse.wst.common.component
deleted file mode 100755
index 72dafadb28..0000000000
--- a/sw/in_progress/pow/ServletPow/eclipse ServletPow/.settings/org.eclipse.wst.common.component
+++ /dev/null
@@ -1,9 +0,0 @@
-
-
-
-
-
-
-
-
-
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/.settings/org.eclipse.wst.common.project.facet.core.xml b/sw/in_progress/pow/ServletPow/eclipse ServletPow/.settings/org.eclipse.wst.common.project.facet.core.xml
deleted file mode 100755
index 53d628c892..0000000000
--- a/sw/in_progress/pow/ServletPow/eclipse ServletPow/.settings/org.eclipse.wst.common.project.facet.core.xml
+++ /dev/null
@@ -1,9 +0,0 @@
-
-
-
-
-
-
-
-
-
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/.settings/org.eclipse.wst.jsdt.ui.superType.container b/sw/in_progress/pow/ServletPow/eclipse ServletPow/.settings/org.eclipse.wst.jsdt.ui.superType.container
deleted file mode 100755
index 3bd5d0a480..0000000000
--- a/sw/in_progress/pow/ServletPow/eclipse ServletPow/.settings/org.eclipse.wst.jsdt.ui.superType.container
+++ /dev/null
@@ -1 +0,0 @@
-org.eclipse.wst.jsdt.launching.baseBrowserLibrary
\ No newline at end of file
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/.settings/org.eclipse.wst.jsdt.ui.superType.name b/sw/in_progress/pow/ServletPow/eclipse ServletPow/.settings/org.eclipse.wst.jsdt.ui.superType.name
deleted file mode 100755
index 05bd71b6ec..0000000000
--- a/sw/in_progress/pow/ServletPow/eclipse ServletPow/.settings/org.eclipse.wst.jsdt.ui.superType.name
+++ /dev/null
@@ -1 +0,0 @@
-Window
\ No newline at end of file
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/design.css b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/design.css
deleted file mode 100755
index c982ac6974..0000000000
--- a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/design.css
+++ /dev/null
@@ -1,220 +0,0 @@
-body
-{
- background-color: #CCFFCC;
- margin: 0%;
- padding: 0%;
- outline: 0%;
- height: 100%;
- overflow:hidden;
- max-width:2048px;
- font-size : 11;
-}
-
-select{
- font-family: Verdana, Geneva, Arial, Helvetica, sans-serif;
- font-size: 11px;
- vertical-align: middle;
- background-color: #F5F5DC;
-}
-input {
- background-color:#F5F5DC;
- font-family: Verdana, Geneva, Arial, Helvetica, sans-serif;
- font-size: 10px;
-}
-
-label {
- font-family: Verdana, Geneva, Arial, Helvetica, sans-serif;
- font-size: 11px;
-}
-
-#welcome
-{
- position:absolute;
- top:0;
- left:0;
- height:3%;
- width: 100%;
- background-color: #EEEEEE;
- text-align: center;
- font-weight:bold;
- font-size : 12;
-}
-#usr
-{
- position:absolute;
- top:0;
- right:6%;
- height:3%;
- width:25%;
- background-color: #EEEEEE;
- text-align: right;
- white-space:nowrap;
-}
-#page
-{
-position:absolute;
-top:3%;
-left:0;
-height:97%;
-width: 100%;
-}
-
-
-#choicePanel{
- position:absolute;
- top:0%;
- left:0%;
- height: 13%;
- width : 15%;
- background-color: #EEEEEE;
- border: 1px solid black;
-}
-
-#aircraftSelector
-{
- text-align: center;
- background-color: #EEEEEE;
-}
-
-#trackdiv
-{
- text-align: center;
- background-color: #EEEEEE;
- -moz-border-radius: 7px 7px 7px 7px;
- -webkit-border-radius: 7px 7px 7px 7px;
- border-radius: 7px 7px 7px 7px;
- -webkit-border-top-left-radius: 7px; /* pour Chrome */
- -webkit-border-top-right-radius: 7px; /* pour Chrome */
- -webkit-border-bottom-left-radius: 7px; /* pour Chrome */
- -webkit-border-bottom-right-radius: 7px; /* pour Chrome */
- width: 140px;
- color: white;
- vertical-align:middle;
-}
-
-#active_block
-{
- font-size:11;
- text-align: center;
-}
-
-#FlightPlan
-{
- overflow:auto;
- position:absolute;
- top:0%;
- left:15%;
- height: 40%;
- width : 14.9%;
- background-color: #EEEEEE;
- border: 1px solid black;
-}
-
-#FlightParams
-{
- position:absolute;
- top:13%;
- left :0%
- text-align: center;
- height: 27%;
- width : 14.9%;
- border: 1px solid black;
- background-color: #EEEEEE;
-}
-
-#map_canvas
-{
- position:absolute;
- top:0;
- left:30%;
- width:70%;
- height: 95%;
- border:1px solid black;
-}
-
-
-#Settings
-{
- overflow:auto;
- position:absolute;
- left:0%;
- top:40%;
- text-align: left;
- height: 55%;
- width: 29.9%;
- background-color: #EEEEEE;
- border: 1px solid black;
-}
-
-#page_setting {
-
-}
-
-#setting {
- width: 400px;
- height: 250px;
- padding: 0.5em;
- font-family: Verdana, Geneva, Arial, Helvetica, sans-serif;
- font-size: 8;
-}
-#param
-{
- overflow:auto;
-}
-
-#NewsDiv
-{
- position:absolute;
- overflow:auto;
- left:0%;
- top:95%;
- height: 5%;
- width: 100%;
- text-align: left;
- vertical-align:top;
- background-color: #EEEEEE;
- border: 1px solid black;
-}
-
-#info
-{
- text-align: left;
- font-family: Verdana, Geneva, Arial, Helvetica, sans-serif;
- font-size: 11px;
-}
-
-#Debug
-{
- overflow:auto;
- position:absolute;
- left:65%;
- top:50%;
- text-align: center;
- height: 50%;
- width: 35%;
- background-color: #EEEEEE;
- border: 1px solid black;
- padding-top:20px;
- font-family: Verdana, Geneva, Arial, Helvetica, sans-serif;
- font-size: 11px;
-}
-
-#title{
- font-weight:bold;
- font-size:18;
- text-align:center;
-}
-
-table.param {
-border:3px solid #6495ed;
-border-collapse:collapse;
-width:90%;
-margin:auto;
-}
-td.param {
-font-family:sans-serif;
-font-size:11px;
-border:1px solid #6495ed;
-padding:5px;
-text-align:left;
-}
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/designHelp.css b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/designHelp.css
deleted file mode 100755
index e8fcfd7a3d..0000000000
--- a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/designHelp.css
+++ /dev/null
@@ -1,21 +0,0 @@
-body
-{
- width: 90%;
- margin-top: 5px;
- margin-bottom: 5px;
- margin-left:5%;
- margin-reight:5%;
- background-color: #CCFFCC;
- text-align:justify;
-}
-
-h3,h2
-{
- color : navy;
- text-align:center;
- text-decoration:underline;
-}
-#problem
-{
- color:red;
-}
\ No newline at end of file
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/designLogOut.css b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/designLogOut.css
deleted file mode 100755
index c6ce91f1e9..0000000000
--- a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/designLogOut.css
+++ /dev/null
@@ -1,12 +0,0 @@
-body
-{
- background-color: #CCFFCC;
- margin-top: 2%;
- padding: 0%;
- outline: 0%;
- height: 100%;
- overflow:hidden;
- max-width:2048px;
- text-align:center;
- font-size:24px;
-}
\ No newline at end of file
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/designWelcome.css b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/designWelcome.css
deleted file mode 100755
index fe096f7074..0000000000
--- a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/designWelcome.css
+++ /dev/null
@@ -1,30 +0,0 @@
-body
-{
- width: 960px;
- margin-top: 100px;
- margin-bottom: 5px;
- background-image: url("Icons/logo.png");
- background-repeat:no-repeat;
- background-color: #CCFFCC;
-
-}
-
-#signIn
-{
- margin: auto;
- width: 300px;
- height: 140px;
- border: 1px solid black;
-}
-
-#log
-{
- margin-left: 30px;
-}
-
-a
-{
- position:absolute;
- right:1%;
- top:80%;
-}
\ No newline at end of file
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/design_admin.css b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/design_admin.css
deleted file mode 100755
index 867056a7a6..0000000000
--- a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/design_admin.css
+++ /dev/null
@@ -1,99 +0,0 @@
-body {
- background-color: #EEEEEE;
- font-family: Verdana, Geneva, Arial, Helvetica, sans-serif;
- font-size: 11px;
-}
-
-select{
- font-family: Verdana, Geneva, Arial, Helvetica, sans-serif;
- font-size: 11px;
- vertical-align: middle;
- background-color: #F5F5DC;
-}
-input {
- border:1px solid black;
- background-color:#F5F5DC;
- font-size: 11px;
-}
-
-label {
- font-family: Verdana, Geneva, Arial, Helvetica, sans-serif;
- font-size: 11px;
-}
-
-span {
- font-family: Verdana, Geneva, Arial, Helvetica, sans-serif;
- font-size: 11px;
-}
-
-td {
- font-family: Verdana, Geneva, Arial, Helvetica, sans-serif;
- font-size: 11px;
-}
-
-#title {
- position : absolute;
- top : 0%;
- left : 0%;
- height : 5%;
- width : 100%;
- font-family: Verdana, Geneva, Arial, Helvetica, sans-serif;
- text-align : right;
- font-size : 11px;
- background-color: #BC8F8F;
- border: 1px solid black;
-}
-
-#page {
- position : absolute;
- top : 15%;
- left : 15%;
- width : 70%;
- height : 65%;
- border: 1px solid black;
- background-color: #EEEEEE;
-}
-
-
-#modifImmat {
- position : absolute;
- top : 15%;
-}
-
-
-#modifPwd {
- position : absolute;
- top : 15%;
-}
-
-
-#modifRight {
- position : absolute;
- top : 15%;
-}
-
-#modifyUser {
- position : absolute;
- top : 15%;
-}
-
-#createUser {
- position : absolute;
- top : 15%;
-}
-
-#mainForm {
- position : absolute;
- top : 15%;
-}
-
-
-#createIvyUser {
- position : absolute;
- top : 15%;
-}
-
-#listIvyUser {
- position : absolute;
- top : 15%;
-}
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/designbackup.css b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/designbackup.css
deleted file mode 100755
index b00585c71d..0000000000
--- a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/designbackup.css
+++ /dev/null
@@ -1,87 +0,0 @@
-body
-{
- background-color: #CCFFCC;
- margin: 0%;
- padding: 0%;
- outline: 0%;
- height: 95%;
-}
-#page
-{
- width: 100%;
- height: 95%;
-}
-
-#logo
-{
- width: 960px;
- height: 100px;
- background-image: url("banniere.jpg");
- background-repeat: repeat-x;
- margin-bottom: 10px;
-}
-
-#leftmenu
-{
- /* float: left; /* Le menu flottera à gauche */
- /* width: 120px; *//* Très important : donner une taille au menu */
-}
-#welcome
-{
- text-align: center;
- background-color: #EEEEEE;
- border: 2px solid black;
- margin-bottom: 10px; /* Pour éviter que les éléments du menu ne soient trop collés */
-}
-#choice
-{
-float: left;
- text-align: center;
- background-color: #EEEEEE;
- border: 2px solid black;
- height: 190px;
- width: 20px;
-}
-#map_canvas
-{
- width: 690px;
- height: 350px;
- margin-left: 125px;
-}
-
-#FlightPlan
-{
- float: right;
- width: 135px; /* Très important : donner une taille au menu */
- height: 345px;
- border: 2px solid black;
- background-color: #EEEEEE;
-}
-
-
-
-#ControlPanel
-{
- float: left;
- text-align: center;
- height: 150px;
- width: 600px;
- border: 2px solid black;
- background-color: #EEEEEE;
-}
-#FlightParams
-{
- text-align: center;
- float: right;
- height: 150px;
- width: 350px;
- border: 2px solid black;
- background-color: #EEEEEE;
-}
-table
-{
- margin-left: 20px;
- margin-bottom: 20px;
-}
-
-
\ No newline at end of file
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/dtree.css b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/dtree.css
deleted file mode 100755
index b201c2fd60..0000000000
--- a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/dtree.css
+++ /dev/null
@@ -1,34 +0,0 @@
-/*--------------------------------------------------|
-| dTree 2.05 | www.destroydrop.com/javascript/tree/ |
-|---------------------------------------------------|
-| Copyright (c) 2002-2003 Geir Landrö |
-|--------------------------------------------------*/
-
-.dtree {
- font-family: Verdana, Geneva, Arial, Helvetica, sans-serif;
- font-size: 11px;
- color: #666;
- white-space: nowrap;
-}
-.dtree img {
- border: 0px;
- vertical-align: middle;
-}
-.dtree a {
- color: #333;
- text-decoration: none;
-}
-.dtree a.node, .dtree a.nodeSel {
- white-space: nowrap;
- padding: 1px 2px 1px 2px;
-}
-.dtree a.node:hover, .dtree a.nodeSel:hover {
- color: #333;
- text-decoration: underline;
-}
-.dtree a.nodeSel {
- background-color: #c0d2ec;
-}
-.dtree .clip {
- overflow: hidden;
-}
\ No newline at end of file
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/tab.css b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/tab.css
deleted file mode 100755
index 2e9043ed76..0000000000
--- a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/tab.css
+++ /dev/null
@@ -1,110 +0,0 @@
-/* $Id: example.css,v 1.5 2006/03/27 02:44:36 pat Exp $ */
-
-/*--------------------------------------------------
- REQUIRED to hide the non-active tab content.
- But do not hide them in the print stylesheet!
- --------------------------------------------------*/
-.tabberlive .tabbertabhide {
- display:none;
-}
-
-/*--------------------------------------------------
- .tabber = before the tabber interface is set up
- .tabberlive = after the tabber interface is set up
- --------------------------------------------------*/
-.tabber {
-}
-.tabberlive {
- margin-top:1em;
-}
-
-/*--------------------------------------------------
- ul.tabbernav = the tab navigation list
- li.tabberactive = the active tab
- --------------------------------------------------*/
-ul.tabbernav
-{
- margin:0;
- padding: 3px 0;
- border-bottom: 1px solid #778;
- font: bold 12px Verdana, sans-serif;
-}
-
-ul.tabbernav li
-{
- list-style: none;
- margin: 0;
- display: inline;
-}
-
-ul.tabbernav li a
-{
- padding: 3px 0.5em;
- margin-left: 3px;
- border: 1px solid #778;
- border-bottom: none;
- background: #DDE;
- text-decoration: none;
-}
-
-ul.tabbernav li a:link { color: #448; }
-ul.tabbernav li a:visited { color: #667; }
-
-ul.tabbernav li a:hover
-{
- color: #000;
- background: #AAE;
- border-color: #227;
-}
-
-ul.tabbernav li.tabberactive a
-{
- background-color: #fff;
- border-bottom: 1px solid #fff;
-}
-
-ul.tabbernav li.tabberactive a:hover
-{
- color: #000;
- background: white;
- border-bottom: 1px solid white;
-}
-
-/*--------------------------------------------------
- .tabbertab = the tab content
- Add style only after the tabber interface is set up (.tabberlive)
- --------------------------------------------------*/
-.tabberlive .tabbertab {
- padding:5px;
- border:1px solid #aaa;
- border-top:0;
-
- /* If you don't want the tab size changing whenever a tab is changed
- you can set a fixed height */
-
- /* height:200px; */
-
- /* If you set a fix height set overflow to auto and you will get a
- scrollbar when necessary */
-
- /* overflow:auto; */
-}
-
-/* If desired, hide the heading since a heading is provided by the tab */
-.tabberlive .tabbertab h2 {
- display:none;
-}
-.tabberlive .tabbertab h3 {
- display:none;
-}
-
-/* Example of using an ID to set different styles for the tabs on the page */
-.tabberlive#tab1 {
-}
-.tabberlive#tab2 {
-}
-.tabberlive#tab2 .tabbertab {
- height:200px;
- overflow:auto;
-}
-
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-anim_basic_16x16.gif b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-anim_basic_16x16.gif
deleted file mode 100755
index 085ccaecaf..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-anim_basic_16x16.gif and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-bg_diagonals-thick_18_b81900_40x40.png b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-bg_diagonals-thick_18_b81900_40x40.png
deleted file mode 100755
index 954e22dbd9..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-bg_diagonals-thick_18_b81900_40x40.png and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-bg_diagonals-thick_20_666666_40x40.png b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-bg_diagonals-thick_20_666666_40x40.png
deleted file mode 100755
index 64ece5707d..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-bg_diagonals-thick_20_666666_40x40.png and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-bg_flat_10_000000_40x100.png b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-bg_flat_10_000000_40x100.png
deleted file mode 100755
index abdc01082b..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-bg_flat_10_000000_40x100.png and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-bg_glass_100_f6f6f6_1x400.png b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-bg_glass_100_f6f6f6_1x400.png
deleted file mode 100755
index 9b383f4d2e..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-bg_glass_100_f6f6f6_1x400.png and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-bg_glass_100_fdf5ce_1x400.png b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-bg_glass_100_fdf5ce_1x400.png
deleted file mode 100755
index a23baad25b..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-bg_glass_100_fdf5ce_1x400.png and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-bg_glass_65_ffffff_1x400.png b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-bg_glass_65_ffffff_1x400.png
deleted file mode 100755
index 42ccba269b..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-bg_glass_65_ffffff_1x400.png and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-bg_gloss-wave_35_f6a828_500x100.png b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-bg_gloss-wave_35_f6a828_500x100.png
deleted file mode 100755
index 39d5824d6a..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-bg_gloss-wave_35_f6a828_500x100.png and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-bg_highlight-soft_100_eeeeee_1x100.png b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-bg_highlight-soft_100_eeeeee_1x100.png
deleted file mode 100755
index f1273672d2..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-bg_highlight-soft_100_eeeeee_1x100.png and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-bg_highlight-soft_75_ffe45c_1x100.png b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-bg_highlight-soft_75_ffe45c_1x100.png
deleted file mode 100755
index 359397acff..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-bg_highlight-soft_75_ffe45c_1x100.png and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-icons_222222_256x240.png b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-icons_222222_256x240.png
deleted file mode 100755
index b273ff111d..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-icons_222222_256x240.png and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-icons_228ef1_256x240.png b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-icons_228ef1_256x240.png
deleted file mode 100755
index c357355aa7..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-icons_228ef1_256x240.png and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-icons_ef8c08_256x240.png b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-icons_ef8c08_256x240.png
deleted file mode 100755
index 85e63e9f60..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-icons_ef8c08_256x240.png and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-icons_ffd27a_256x240.png b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-icons_ffd27a_256x240.png
deleted file mode 100755
index e117effa3d..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-icons_ffd27a_256x240.png and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-icons_ffffff_256x240.png b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-icons_ffffff_256x240.png
deleted file mode 100755
index 42f8f992c7..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/images/ui-icons_ffffff_256x240.png and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/jquery-ui-1.8.2.custom.css b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/jquery-ui-1.8.2.custom.css
deleted file mode 100755
index 312dd8043e..0000000000
--- a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/CSS/ui-lightness/jquery-ui-1.8.2.custom.css
+++ /dev/null
@@ -1,489 +0,0 @@
-/*
-* jQuery UI CSS Framework
-* Copyright (c) 2010 AUTHORS.txt (http://jqueryui.com/about)
-* Dual licensed under the MIT (MIT-LICENSE.txt) and GPL (GPL-LICENSE.txt) licenses.
-*/
-
-/* Layout helpers
-----------------------------------*/
-.ui-helper-hidden { display: none; }
-.ui-helper-hidden-accessible { position: absolute; left: -99999999px; }
-.ui-helper-reset { margin: 0; padding: 0; border: 0; outline: 0; line-height: 1.3; text-decoration: none; font-size: 100%; list-style: none; }
-.ui-helper-clearfix:after { content: "."; display: block; height: 0; clear: both; visibility: hidden; }
-.ui-helper-clearfix { display: inline-block; }
-/* required comment for clearfix to work in Opera \*/
-* html .ui-helper-clearfix { height:1%; }
-.ui-helper-clearfix { display:block; }
-/* end clearfix */
-.ui-helper-zfix { width: 100%; height: 100%; top: 0; left: 0; position: absolute; opacity: 0; filter:Alpha(Opacity=0); }
-
-
-/* Interaction Cues
-----------------------------------*/
-.ui-state-disabled { cursor: default !important; }
-
-
-/* Icons
-----------------------------------*/
-
-/* states and images */
-.ui-icon { display: block; text-indent: -99999px; overflow: hidden; background-repeat: no-repeat; }
-
-
-/* Misc visuals
-----------------------------------*/
-
-/* Overlays */
-.ui-widget-overlay { position: absolute; top: 0; left: 0; width: 100%; height: 100%; }
-
-
-/*
-* jQuery UI CSS Framework
-* Copyright (c) 2010 AUTHORS.txt (http://jqueryui.com/about)
-* Dual licensed under the MIT (MIT-LICENSE.txt) and GPL (GPL-LICENSE.txt) licenses.
-* To view and modify this theme, visit http://jqueryui.com/themeroller/?ffDefault=Trebuchet%20MS,%20Tahoma,%20Verdana,%20Arial,%20sans-serif&fwDefault=bold&fsDefault=1.1em&cornerRadius=4px&bgColorHeader=f6a828&bgTextureHeader=12_gloss_wave.png&bgImgOpacityHeader=35&borderColorHeader=e78f08&fcHeader=ffffff&iconColorHeader=ffffff&bgColorContent=eeeeee&bgTextureContent=03_highlight_soft.png&bgImgOpacityContent=100&borderColorContent=dddddd&fcContent=333333&iconColorContent=222222&bgColorDefault=f6f6f6&bgTextureDefault=02_glass.png&bgImgOpacityDefault=100&borderColorDefault=cccccc&fcDefault=1c94c4&iconColorDefault=ef8c08&bgColorHover=fdf5ce&bgTextureHover=02_glass.png&bgImgOpacityHover=100&borderColorHover=fbcb09&fcHover=c77405&iconColorHover=ef8c08&bgColorActive=ffffff&bgTextureActive=02_glass.png&bgImgOpacityActive=65&borderColorActive=fbd850&fcActive=eb8f00&iconColorActive=ef8c08&bgColorHighlight=ffe45c&bgTextureHighlight=03_highlight_soft.png&bgImgOpacityHighlight=75&borderColorHighlight=fed22f&fcHighlight=363636&iconColorHighlight=228ef1&bgColorError=b81900&bgTextureError=08_diagonals_thick.png&bgImgOpacityError=18&borderColorError=cd0a0a&fcError=ffffff&iconColorError=ffd27a&bgColorOverlay=666666&bgTextureOverlay=08_diagonals_thick.png&bgImgOpacityOverlay=20&opacityOverlay=50&bgColorShadow=000000&bgTextureShadow=01_flat.png&bgImgOpacityShadow=10&opacityShadow=20&thicknessShadow=5px&offsetTopShadow=-5px&offsetLeftShadow=-5px&cornerRadiusShadow=5px
-*/
-
-
-/* Component containers
-----------------------------------*/
-.ui-widget { font-family: Trebuchet MS, Tahoma, Verdana, Arial, sans-serif; font-size: 1.1em; }
-.ui-widget .ui-widget { font-size: 1em; }
-.ui-widget input, .ui-widget select, .ui-widget textarea, .ui-widget button { font-family: Trebuchet MS, Tahoma, Verdana, Arial, sans-serif; font-size: 1em; }
-.ui-widget-content { border: 1px solid #dddddd; background: #eeeeee url(images/ui-bg_highlight-soft_100_eeeeee_1x100.png) 50% top repeat-x; color: #333333; }
-.ui-widget-content a { color: #333333; }
-.ui-widget-header { border: 1px solid #e78f08; background: #f6a828 url(images/ui-bg_gloss-wave_35_f6a828_500x100.png) 50% 50% repeat-x; color: #ffffff; font-weight: bold; }
-.ui-widget-header a { color: #ffffff; }
-
-/* Interaction states
-----------------------------------*/
-.ui-state-default, .ui-widget-content .ui-state-default, .ui-widget-header .ui-state-default { border: 1px solid #cccccc; background: #f6f6f6 url(images/ui-bg_glass_100_f6f6f6_1x400.png) 50% 50% repeat-x; font-weight: bold; color: #1c94c4; }
-.ui-state-default a, .ui-state-default a:link, .ui-state-default a:visited { color: #1c94c4; text-decoration: none; }
-.ui-state-hover, .ui-widget-content .ui-state-hover, .ui-widget-header .ui-state-hover, .ui-state-focus, .ui-widget-content .ui-state-focus, .ui-widget-header .ui-state-focus { border: 1px solid #fbcb09; background: #fdf5ce url(images/ui-bg_glass_100_fdf5ce_1x400.png) 50% 50% repeat-x; font-weight: bold; color: #c77405; }
-.ui-state-hover a, .ui-state-hover a:hover { color: #c77405; text-decoration: none; }
-.ui-state-active, .ui-widget-content .ui-state-active, .ui-widget-header .ui-state-active { border: 1px solid #fbd850; background: #ffffff url(images/ui-bg_glass_65_ffffff_1x400.png) 50% 50% repeat-x; font-weight: bold; color: #eb8f00; }
-.ui-state-active a, .ui-state-active a:link, .ui-state-active a:visited { color: #eb8f00; text-decoration: none; }
-.ui-widget :active { outline: none; }
-
-/* Interaction Cues
-----------------------------------*/
-.ui-state-highlight, .ui-widget-content .ui-state-highlight, .ui-widget-header .ui-state-highlight {border: 1px solid #fed22f; background: #ffe45c url(images/ui-bg_highlight-soft_75_ffe45c_1x100.png) 50% top repeat-x; color: #363636; }
-.ui-state-highlight a, .ui-widget-content .ui-state-highlight a,.ui-widget-header .ui-state-highlight a { color: #363636; }
-.ui-state-error, .ui-widget-content .ui-state-error, .ui-widget-header .ui-state-error {border: 1px solid #cd0a0a; background: #b81900 url(images/ui-bg_diagonals-thick_18_b81900_40x40.png) 50% 50% repeat; color: #ffffff; }
-.ui-state-error a, .ui-widget-content .ui-state-error a, .ui-widget-header .ui-state-error a { color: #ffffff; }
-.ui-state-error-text, .ui-widget-content .ui-state-error-text, .ui-widget-header .ui-state-error-text { color: #ffffff; }
-.ui-priority-primary, .ui-widget-content .ui-priority-primary, .ui-widget-header .ui-priority-primary { font-weight: bold; }
-.ui-priority-secondary, .ui-widget-content .ui-priority-secondary, .ui-widget-header .ui-priority-secondary { opacity: .7; filter:Alpha(Opacity=70); font-weight: normal; }
-.ui-state-disabled, .ui-widget-content .ui-state-disabled, .ui-widget-header .ui-state-disabled { opacity: .35; filter:Alpha(Opacity=35); background-image: none; }
-
-/* Icons
-----------------------------------*/
-
-/* states and images */
-.ui-icon { width: 16px; height: 16px; background-image: url(images/ui-icons_222222_256x240.png); }
-.ui-widget-content .ui-icon {background-image: url(images/ui-icons_222222_256x240.png); }
-.ui-widget-header .ui-icon {background-image: url(images/ui-icons_ffffff_256x240.png); }
-.ui-state-default .ui-icon { background-image: url(images/ui-icons_ef8c08_256x240.png); }
-.ui-state-hover .ui-icon, .ui-state-focus .ui-icon {background-image: url(images/ui-icons_ef8c08_256x240.png); }
-.ui-state-active .ui-icon {background-image: url(images/ui-icons_ef8c08_256x240.png); }
-.ui-state-highlight .ui-icon {background-image: url(images/ui-icons_228ef1_256x240.png); }
-.ui-state-error .ui-icon, .ui-state-error-text .ui-icon {background-image: url(images/ui-icons_ffd27a_256x240.png); }
-
-/* positioning */
-.ui-icon-carat-1-n { background-position: 0 0; }
-.ui-icon-carat-1-ne { background-position: -16px 0; }
-.ui-icon-carat-1-e { background-position: -32px 0; }
-.ui-icon-carat-1-se { background-position: -48px 0; }
-.ui-icon-carat-1-s { background-position: -64px 0; }
-.ui-icon-carat-1-sw { background-position: -80px 0; }
-.ui-icon-carat-1-w { background-position: -96px 0; }
-.ui-icon-carat-1-nw { background-position: -112px 0; }
-.ui-icon-carat-2-n-s { background-position: -128px 0; }
-.ui-icon-carat-2-e-w { background-position: -144px 0; }
-.ui-icon-triangle-1-n { background-position: 0 -16px; }
-.ui-icon-triangle-1-ne { background-position: -16px -16px; }
-.ui-icon-triangle-1-e { background-position: -32px -16px; }
-.ui-icon-triangle-1-se { background-position: -48px -16px; }
-.ui-icon-triangle-1-s { background-position: -64px -16px; }
-.ui-icon-triangle-1-sw { background-position: -80px -16px; }
-.ui-icon-triangle-1-w { background-position: -96px -16px; }
-.ui-icon-triangle-1-nw { background-position: -112px -16px; }
-.ui-icon-triangle-2-n-s { background-position: -128px -16px; }
-.ui-icon-triangle-2-e-w { background-position: -144px -16px; }
-.ui-icon-arrow-1-n { background-position: 0 -32px; }
-.ui-icon-arrow-1-ne { background-position: -16px -32px; }
-.ui-icon-arrow-1-e { background-position: -32px -32px; }
-.ui-icon-arrow-1-se { background-position: -48px -32px; }
-.ui-icon-arrow-1-s { background-position: -64px -32px; }
-.ui-icon-arrow-1-sw { background-position: -80px -32px; }
-.ui-icon-arrow-1-w { background-position: -96px -32px; }
-.ui-icon-arrow-1-nw { background-position: -112px -32px; }
-.ui-icon-arrow-2-n-s { background-position: -128px -32px; }
-.ui-icon-arrow-2-ne-sw { background-position: -144px -32px; }
-.ui-icon-arrow-2-e-w { background-position: -160px -32px; }
-.ui-icon-arrow-2-se-nw { background-position: -176px -32px; }
-.ui-icon-arrowstop-1-n { background-position: -192px -32px; }
-.ui-icon-arrowstop-1-e { background-position: -208px -32px; }
-.ui-icon-arrowstop-1-s { background-position: -224px -32px; }
-.ui-icon-arrowstop-1-w { background-position: -240px -32px; }
-.ui-icon-arrowthick-1-n { background-position: 0 -48px; }
-.ui-icon-arrowthick-1-ne { background-position: -16px -48px; }
-.ui-icon-arrowthick-1-e { background-position: -32px -48px; }
-.ui-icon-arrowthick-1-se { background-position: -48px -48px; }
-.ui-icon-arrowthick-1-s { background-position: -64px -48px; }
-.ui-icon-arrowthick-1-sw { background-position: -80px -48px; }
-.ui-icon-arrowthick-1-w { background-position: -96px -48px; }
-.ui-icon-arrowthick-1-nw { background-position: -112px -48px; }
-.ui-icon-arrowthick-2-n-s { background-position: -128px -48px; }
-.ui-icon-arrowthick-2-ne-sw { background-position: -144px -48px; }
-.ui-icon-arrowthick-2-e-w { background-position: -160px -48px; }
-.ui-icon-arrowthick-2-se-nw { background-position: -176px -48px; }
-.ui-icon-arrowthickstop-1-n { background-position: -192px -48px; }
-.ui-icon-arrowthickstop-1-e { background-position: -208px -48px; }
-.ui-icon-arrowthickstop-1-s { background-position: -224px -48px; }
-.ui-icon-arrowthickstop-1-w { background-position: -240px -48px; }
-.ui-icon-arrowreturnthick-1-w { background-position: 0 -64px; }
-.ui-icon-arrowreturnthick-1-n { background-position: -16px -64px; }
-.ui-icon-arrowreturnthick-1-e { background-position: -32px -64px; }
-.ui-icon-arrowreturnthick-1-s { background-position: -48px -64px; }
-.ui-icon-arrowreturn-1-w { background-position: -64px -64px; }
-.ui-icon-arrowreturn-1-n { background-position: -80px -64px; }
-.ui-icon-arrowreturn-1-e { background-position: -96px -64px; }
-.ui-icon-arrowreturn-1-s { background-position: -112px -64px; }
-.ui-icon-arrowrefresh-1-w { background-position: -128px -64px; }
-.ui-icon-arrowrefresh-1-n { background-position: -144px -64px; }
-.ui-icon-arrowrefresh-1-e { background-position: -160px -64px; }
-.ui-icon-arrowrefresh-1-s { background-position: -176px -64px; }
-.ui-icon-arrow-4 { background-position: 0 -80px; }
-.ui-icon-arrow-4-diag { background-position: -16px -80px; }
-.ui-icon-extlink { background-position: -32px -80px; }
-.ui-icon-newwin { background-position: -48px -80px; }
-.ui-icon-refresh { background-position: -64px -80px; }
-.ui-icon-shuffle { background-position: -80px -80px; }
-.ui-icon-transfer-e-w { background-position: -96px -80px; }
-.ui-icon-transferthick-e-w { background-position: -112px -80px; }
-.ui-icon-folder-collapsed { background-position: 0 -96px; }
-.ui-icon-folder-open { background-position: -16px -96px; }
-.ui-icon-document { background-position: -32px -96px; }
-.ui-icon-document-b { background-position: -48px -96px; }
-.ui-icon-note { background-position: -64px -96px; }
-.ui-icon-mail-closed { background-position: -80px -96px; }
-.ui-icon-mail-open { background-position: -96px -96px; }
-.ui-icon-suitcase { background-position: -112px -96px; }
-.ui-icon-comment { background-position: -128px -96px; }
-.ui-icon-person { background-position: -144px -96px; }
-.ui-icon-print { background-position: -160px -96px; }
-.ui-icon-trash { background-position: -176px -96px; }
-.ui-icon-locked { background-position: -192px -96px; }
-.ui-icon-unlocked { background-position: -208px -96px; }
-.ui-icon-bookmark { background-position: -224px -96px; }
-.ui-icon-tag { background-position: -240px -96px; }
-.ui-icon-home { background-position: 0 -112px; }
-.ui-icon-flag { background-position: -16px -112px; }
-.ui-icon-calendar { background-position: -32px -112px; }
-.ui-icon-cart { background-position: -48px -112px; }
-.ui-icon-pencil { background-position: -64px -112px; }
-.ui-icon-clock { background-position: -80px -112px; }
-.ui-icon-disk { background-position: -96px -112px; }
-.ui-icon-calculator { background-position: -112px -112px; }
-.ui-icon-zoomin { background-position: -128px -112px; }
-.ui-icon-zoomout { background-position: -144px -112px; }
-.ui-icon-search { background-position: -160px -112px; }
-.ui-icon-wrench { background-position: -176px -112px; }
-.ui-icon-gear { background-position: -192px -112px; }
-.ui-icon-heart { background-position: -208px -112px; }
-.ui-icon-star { background-position: -224px -112px; }
-.ui-icon-link { background-position: -240px -112px; }
-.ui-icon-cancel { background-position: 0 -128px; }
-.ui-icon-plus { background-position: -16px -128px; }
-.ui-icon-plusthick { background-position: -32px -128px; }
-.ui-icon-minus { background-position: -48px -128px; }
-.ui-icon-minusthick { background-position: -64px -128px; }
-.ui-icon-close { background-position: -80px -128px; }
-.ui-icon-closethick { background-position: -96px -128px; }
-.ui-icon-key { background-position: -112px -128px; }
-.ui-icon-lightbulb { background-position: -128px -128px; }
-.ui-icon-scissors { background-position: -144px -128px; }
-.ui-icon-clipboard { background-position: -160px -128px; }
-.ui-icon-copy { background-position: -176px -128px; }
-.ui-icon-contact { background-position: -192px -128px; }
-.ui-icon-image { background-position: -208px -128px; }
-.ui-icon-video { background-position: -224px -128px; }
-.ui-icon-script { background-position: -240px -128px; }
-.ui-icon-alert { background-position: 0 -144px; }
-.ui-icon-info { background-position: -16px -144px; }
-.ui-icon-notice { background-position: -32px -144px; }
-.ui-icon-help { background-position: -48px -144px; }
-.ui-icon-check { background-position: -64px -144px; }
-.ui-icon-bullet { background-position: -80px -144px; }
-.ui-icon-radio-off { background-position: -96px -144px; }
-.ui-icon-radio-on { background-position: -112px -144px; }
-.ui-icon-pin-w { background-position: -128px -144px; }
-.ui-icon-pin-s { background-position: -144px -144px; }
-.ui-icon-play { background-position: 0 -160px; }
-.ui-icon-pause { background-position: -16px -160px; }
-.ui-icon-seek-next { background-position: -32px -160px; }
-.ui-icon-seek-prev { background-position: -48px -160px; }
-.ui-icon-seek-end { background-position: -64px -160px; }
-.ui-icon-seek-start { background-position: -80px -160px; }
-/* ui-icon-seek-first is deprecated, use ui-icon-seek-start instead */
-.ui-icon-seek-first { background-position: -80px -160px; }
-.ui-icon-stop { background-position: -96px -160px; }
-.ui-icon-eject { background-position: -112px -160px; }
-.ui-icon-volume-off { background-position: -128px -160px; }
-.ui-icon-volume-on { background-position: -144px -160px; }
-.ui-icon-power { background-position: 0 -176px; }
-.ui-icon-signal-diag { background-position: -16px -176px; }
-.ui-icon-signal { background-position: -32px -176px; }
-.ui-icon-battery-0 { background-position: -48px -176px; }
-.ui-icon-battery-1 { background-position: -64px -176px; }
-.ui-icon-battery-2 { background-position: -80px -176px; }
-.ui-icon-battery-3 { background-position: -96px -176px; }
-.ui-icon-circle-plus { background-position: 0 -192px; }
-.ui-icon-circle-minus { background-position: -16px -192px; }
-.ui-icon-circle-close { background-position: -32px -192px; }
-.ui-icon-circle-triangle-e { background-position: -48px -192px; }
-.ui-icon-circle-triangle-s { background-position: -64px -192px; }
-.ui-icon-circle-triangle-w { background-position: -80px -192px; }
-.ui-icon-circle-triangle-n { background-position: -96px -192px; }
-.ui-icon-circle-arrow-e { background-position: -112px -192px; }
-.ui-icon-circle-arrow-s { background-position: -128px -192px; }
-.ui-icon-circle-arrow-w { background-position: -144px -192px; }
-.ui-icon-circle-arrow-n { background-position: -160px -192px; }
-.ui-icon-circle-zoomin { background-position: -176px -192px; }
-.ui-icon-circle-zoomout { background-position: -192px -192px; }
-.ui-icon-circle-check { background-position: -208px -192px; }
-.ui-icon-circlesmall-plus { background-position: 0 -208px; }
-.ui-icon-circlesmall-minus { background-position: -16px -208px; }
-.ui-icon-circlesmall-close { background-position: -32px -208px; }
-.ui-icon-squaresmall-plus { background-position: -48px -208px; }
-.ui-icon-squaresmall-minus { background-position: -64px -208px; }
-.ui-icon-squaresmall-close { background-position: -80px -208px; }
-.ui-icon-grip-dotted-vertical { background-position: 0 -224px; }
-.ui-icon-grip-dotted-horizontal { background-position: -16px -224px; }
-.ui-icon-grip-solid-vertical { background-position: -32px -224px; }
-.ui-icon-grip-solid-horizontal { background-position: -48px -224px; }
-.ui-icon-gripsmall-diagonal-se { background-position: -64px -224px; }
-.ui-icon-grip-diagonal-se { background-position: -80px -224px; }
-
-
-/* Misc visuals
-----------------------------------*/
-
-/* Corner radius */
-.ui-corner-tl { -moz-border-radius-topleft: 4px; -webkit-border-top-left-radius: 4px; border-top-left-radius: 4px; }
-.ui-corner-tr { -moz-border-radius-topright: 4px; -webkit-border-top-right-radius: 4px; border-top-right-radius: 4px; }
-.ui-corner-bl { -moz-border-radius-bottomleft: 4px; -webkit-border-bottom-left-radius: 4px; border-bottom-left-radius: 4px; }
-.ui-corner-br { -moz-border-radius-bottomright: 4px; -webkit-border-bottom-right-radius: 4px; border-bottom-right-radius: 4px; }
-.ui-corner-top { -moz-border-radius-topleft: 4px; -webkit-border-top-left-radius: 4px; border-top-left-radius: 4px; -moz-border-radius-topright: 4px; -webkit-border-top-right-radius: 4px; border-top-right-radius: 4px; }
-.ui-corner-bottom { -moz-border-radius-bottomleft: 4px; -webkit-border-bottom-left-radius: 4px; border-bottom-left-radius: 4px; -moz-border-radius-bottomright: 4px; -webkit-border-bottom-right-radius: 4px; border-bottom-right-radius: 4px; }
-.ui-corner-right { -moz-border-radius-topright: 4px; -webkit-border-top-right-radius: 4px; border-top-right-radius: 4px; -moz-border-radius-bottomright: 4px; -webkit-border-bottom-right-radius: 4px; border-bottom-right-radius: 4px; }
-.ui-corner-left { -moz-border-radius-topleft: 4px; -webkit-border-top-left-radius: 4px; border-top-left-radius: 4px; -moz-border-radius-bottomleft: 4px; -webkit-border-bottom-left-radius: 4px; border-bottom-left-radius: 4px; }
-.ui-corner-all { -moz-border-radius: 4px; -webkit-border-radius: 4px; border-radius: 4px; }
-
-/* Overlays */
-.ui-widget-overlay { background: #666666 url(images/ui-bg_diagonals-thick_20_666666_40x40.png) 50% 50% repeat; opacity: .50;filter:Alpha(Opacity=50); }
-.ui-widget-shadow { margin: -5px 0 0 -5px; padding: 5px; background: #000000 url(images/ui-bg_flat_10_000000_40x100.png) 50% 50% repeat-x; opacity: .20;filter:Alpha(Opacity=20); -moz-border-radius: 5px; -webkit-border-radius: 5px; border-radius: 5px; }/* Resizable
-----------------------------------*/
-.ui-resizable { position: relative;}
-.ui-resizable-handle { position: absolute;font-size: 0.1px;z-index: 99999; display: block;}
-.ui-resizable-disabled .ui-resizable-handle, .ui-resizable-autohide .ui-resizable-handle { display: none; }
-.ui-resizable-n { cursor: n-resize; height: 7px; width: 100%; top: -5px; left: 0; }
-.ui-resizable-s { cursor: s-resize; height: 7px; width: 100%; bottom: -5px; left: 0; }
-.ui-resizable-e { cursor: e-resize; width: 7px; right: -5px; top: 0; height: 100%; }
-.ui-resizable-w { cursor: w-resize; width: 7px; left: -5px; top: 0; height: 100%; }
-.ui-resizable-se { cursor: se-resize; width: 12px; height: 12px; right: 1px; bottom: 1px; }
-.ui-resizable-sw { cursor: sw-resize; width: 9px; height: 9px; left: -5px; bottom: -5px; }
-.ui-resizable-nw { cursor: nw-resize; width: 9px; height: 9px; left: -5px; top: -5px; }
-.ui-resizable-ne { cursor: ne-resize; width: 9px; height: 9px; right: -5px; top: -5px;}/* Selectable
-----------------------------------*/
-.ui-selectable-helper { border:1px dotted black }
-/* Accordion
-----------------------------------*/
-.ui-accordion .ui-accordion-header { cursor: pointer; position: relative; margin-top: 1px; zoom: 1; }
-.ui-accordion .ui-accordion-li-fix { display: inline; }
-.ui-accordion .ui-accordion-header-active { border-bottom: 0 !important; }
-.ui-accordion .ui-accordion-header a { display: block; font-size: 1em; padding: .5em .5em .5em .7em; }
-/* IE7-/Win - Fix extra vertical space in lists */
-.ui-accordion a { zoom: 1; }
-.ui-accordion-icons .ui-accordion-header a { padding-left: 2.2em; }
-.ui-accordion .ui-accordion-header .ui-icon { position: absolute; left: .5em; top: 50%; margin-top: -8px; }
-.ui-accordion .ui-accordion-content { padding: 1em 2.2em; border-top: 0; margin-top: -2px; position: relative; top: 1px; margin-bottom: 2px; overflow: auto; display: none; zoom: 1; }
-.ui-accordion .ui-accordion-content-active { display: block; }/* Autocomplete
-----------------------------------*/
-.ui-autocomplete { position: absolute; cursor: default; }
-.ui-autocomplete-loading { background: white url('images/ui-anim_basic_16x16.gif') right center no-repeat; }
-
-/* workarounds */
-* html .ui-autocomplete { width:1px; } /* without this, the menu expands to 100% in IE6 */
-
-/* Menu
-----------------------------------*/
-.ui-menu {
- list-style:none;
- padding: 2px;
- margin: 0;
- display:block;
-}
-.ui-menu .ui-menu {
- margin-top: -3px;
-}
-.ui-menu .ui-menu-item {
- margin:0;
- padding: 0;
- zoom: 1;
- float: left;
- clear: left;
- width: 100%;
-}
-.ui-menu .ui-menu-item a {
- text-decoration:none;
- display:block;
- padding:.2em .4em;
- line-height:1.5;
- zoom:1;
-}
-.ui-menu .ui-menu-item a.ui-state-hover,
-.ui-menu .ui-menu-item a.ui-state-active {
- font-weight: normal;
- margin: -1px;
-}
-/* Button
-----------------------------------*/
-
-.ui-button { display: inline-block; position: relative; padding: 0; margin-right: .1em; text-decoration: none !important; cursor: pointer; text-align: center; zoom: 1; overflow: visible; } /* the overflow property removes extra width in IE */
-.ui-button-icon-only { width: 2.2em; } /* to make room for the icon, a width needs to be set here */
-button.ui-button-icon-only { width: 2.4em; } /* button elements seem to need a little more width */
-.ui-button-icons-only { width: 3.4em; }
-button.ui-button-icons-only { width: 3.7em; }
-
-/*button text element */
-.ui-button .ui-button-text { display: block; line-height: 1.4; }
-.ui-button-text-only .ui-button-text { padding: .4em 1em; }
-.ui-button-icon-only .ui-button-text, .ui-button-icons-only .ui-button-text { padding: .4em; text-indent: -9999999px; }
-.ui-button-text-icon .ui-button-text, .ui-button-text-icons .ui-button-text { padding: .4em 1em .4em 2.1em; }
-.ui-button-text-icons .ui-button-text { padding-left: 2.1em; padding-right: 2.1em; }
-/* no icon support for input elements, provide padding by default */
-input.ui-button { padding: .4em 1em; }
-
-/*button icon element(s) */
-.ui-button-icon-only .ui-icon, .ui-button-text-icon .ui-icon, .ui-button-text-icons .ui-icon, .ui-button-icons-only .ui-icon { position: absolute; top: 50%; margin-top: -8px; }
-.ui-button-icon-only .ui-icon { left: 50%; margin-left: -8px; }
-.ui-button-text-icon .ui-button-icon-primary, .ui-button-text-icons .ui-button-icon-primary, .ui-button-icons-only .ui-button-icon-primary { left: .5em; }
-.ui-button-text-icons .ui-button-icon-secondary, .ui-button-icons-only .ui-button-icon-secondary { right: .5em; }
-
-/*button sets*/
-.ui-buttonset { margin-right: 7px; }
-.ui-buttonset .ui-button { margin-left: 0; margin-right: -.3em; }
-
-/* workarounds */
-button.ui-button::-moz-focus-inner { border: 0; padding: 0; } /* reset extra padding in Firefox */
-
-
-
-
-
-/* Dialog
-----------------------------------*/
-.ui-dialog { position: absolute; padding: .2em; width: 300px; overflow: hidden; }
-.ui-dialog .ui-dialog-titlebar { padding: .5em 1em .3em; position: relative; }
-.ui-dialog .ui-dialog-title { float: left; margin: .1em 16px .2em 0; }
-.ui-dialog .ui-dialog-titlebar-close { position: absolute; right: .3em; top: 50%; width: 19px; margin: -10px 0 0 0; padding: 1px; height: 18px; }
-.ui-dialog .ui-dialog-titlebar-close span { display: block; margin: 1px; }
-.ui-dialog .ui-dialog-titlebar-close:hover, .ui-dialog .ui-dialog-titlebar-close:focus { padding: 0; }
-.ui-dialog .ui-dialog-content { border: 0; padding: .5em 1em; background: none; overflow: auto; zoom: 1; }
-.ui-dialog .ui-dialog-buttonpane { text-align: left; border-width: 1px 0 0 0; background-image: none; margin: .5em 0 0 0; padding: .3em 1em .5em .4em; }
-.ui-dialog .ui-dialog-buttonpane button { float: right; margin: .5em .4em .5em 0; cursor: pointer; padding: .2em .6em .3em .6em; line-height: 1.4em; width:auto; overflow:visible; }
-.ui-dialog .ui-resizable-se { width: 14px; height: 14px; right: 3px; bottom: 3px; }
-.ui-draggable .ui-dialog-titlebar { cursor: move; }
-/* Slider
-----------------------------------*/
-.ui-slider { position: relative; text-align: left; }
-.ui-slider .ui-slider-handle { position: absolute; z-index: 2; width: 0.8em; height: 0.8em; cursor: default; }
-.ui-slider .ui-slider-range { position: absolute; z-index: 1; font-size: .7em; display: block; border: 0; background-position: 0 0; }
-
-.ui-slider-horizontal { height: .8em; }
-.ui-slider-horizontal .ui-slider-handle { top: -.3em; margin-left: -.6em; }
-.ui-slider-horizontal .ui-slider-range { top: 0; height: 100%; }
-.ui-slider-horizontal .ui-slider-range-min { left: 0; }
-.ui-slider-horizontal .ui-slider-range-max { right: 0; }
-
-.ui-slider-vertical { width: .8em; height: 100px; }
-.ui-slider-vertical .ui-slider-handle { left: -.3em; margin-left: 0; margin-bottom: -.6em; }
-.ui-slider-vertical .ui-slider-range { left: 0; width: 100%; }
-.ui-slider-vertical .ui-slider-range-min { bottom: 0; }
-.ui-slider-vertical .ui-slider-range-max { top: 0; }/* Tabs
-----------------------------------*/
-.ui-tabs { position: relative; padding: .2em; zoom: 1; } /* position: relative prevents IE scroll bug (element with position: relative inside container with overflow: auto appear as "fixed") */
-.ui-tabs .ui-tabs-nav { margin: 0; padding: .2em .2em 0; }
-.ui-tabs .ui-tabs-nav li { list-style: none; float: left; position: relative; top: 1px; margin: 0 .2em 1px 0; border-bottom: 0 !important; padding: 0; white-space: nowrap; }
-.ui-tabs .ui-tabs-nav li a { float: left; padding: .5em 1em; text-decoration: none; }
-.ui-tabs .ui-tabs-nav li.ui-tabs-selected { margin-bottom: 0; padding-bottom: 1px; }
-.ui-tabs .ui-tabs-nav li.ui-tabs-selected a, .ui-tabs .ui-tabs-nav li.ui-state-disabled a, .ui-tabs .ui-tabs-nav li.ui-state-processing a { cursor: text; }
-.ui-tabs .ui-tabs-nav li a, .ui-tabs.ui-tabs-collapsible .ui-tabs-nav li.ui-tabs-selected a { cursor: pointer; } /* first selector in group seems obsolete, but required to overcome bug in Opera applying cursor: text overall if defined elsewhere... */
-.ui-tabs .ui-tabs-panel { display: block; border-width: 0; padding: 1em 1.4em; background: none; }
-.ui-tabs .ui-tabs-hide { display: none !important; }
-/* Datepicker
-----------------------------------*/
-.ui-datepicker { width: 17em; padding: .2em .2em 0; }
-.ui-datepicker .ui-datepicker-header { position:relative; padding:.2em 0; }
-.ui-datepicker .ui-datepicker-prev, .ui-datepicker .ui-datepicker-next { position:absolute; top: 2px; width: 1.8em; height: 1.8em; }
-.ui-datepicker .ui-datepicker-prev-hover, .ui-datepicker .ui-datepicker-next-hover { top: 1px; }
-.ui-datepicker .ui-datepicker-prev { left:2px; }
-.ui-datepicker .ui-datepicker-next { right:2px; }
-.ui-datepicker .ui-datepicker-prev-hover { left:1px; }
-.ui-datepicker .ui-datepicker-next-hover { right:1px; }
-.ui-datepicker .ui-datepicker-prev span, .ui-datepicker .ui-datepicker-next span { display: block; position: absolute; left: 50%; margin-left: -8px; top: 50%; margin-top: -8px; }
-.ui-datepicker .ui-datepicker-title { margin: 0 2.3em; line-height: 1.8em; text-align: center; }
-.ui-datepicker .ui-datepicker-title select { font-size:1em; margin:1px 0; }
-.ui-datepicker select.ui-datepicker-month-year {width: 100%;}
-.ui-datepicker select.ui-datepicker-month,
-.ui-datepicker select.ui-datepicker-year { width: 49%;}
-.ui-datepicker table {width: 100%; font-size: .9em; border-collapse: collapse; margin:0 0 .4em; }
-.ui-datepicker th { padding: .7em .3em; text-align: center; font-weight: bold; border: 0; }
-.ui-datepicker td { border: 0; padding: 1px; }
-.ui-datepicker td span, .ui-datepicker td a { display: block; padding: .2em; text-align: right; text-decoration: none; }
-.ui-datepicker .ui-datepicker-buttonpane { background-image: none; margin: .7em 0 0 0; padding:0 .2em; border-left: 0; border-right: 0; border-bottom: 0; }
-.ui-datepicker .ui-datepicker-buttonpane button { float: right; margin: .5em .2em .4em; cursor: pointer; padding: .2em .6em .3em .6em; width:auto; overflow:visible; }
-.ui-datepicker .ui-datepicker-buttonpane button.ui-datepicker-current { float:left; }
-
-/* with multiple calendars */
-.ui-datepicker.ui-datepicker-multi { width:auto; }
-.ui-datepicker-multi .ui-datepicker-group { float:left; }
-.ui-datepicker-multi .ui-datepicker-group table { width:95%; margin:0 auto .4em; }
-.ui-datepicker-multi-2 .ui-datepicker-group { width:50%; }
-.ui-datepicker-multi-3 .ui-datepicker-group { width:33.3%; }
-.ui-datepicker-multi-4 .ui-datepicker-group { width:25%; }
-.ui-datepicker-multi .ui-datepicker-group-last .ui-datepicker-header { border-left-width:0; }
-.ui-datepicker-multi .ui-datepicker-group-middle .ui-datepicker-header { border-left-width:0; }
-.ui-datepicker-multi .ui-datepicker-buttonpane { clear:left; }
-.ui-datepicker-row-break { clear:both; width:100%; }
-
-/* RTL support */
-.ui-datepicker-rtl { direction: rtl; }
-.ui-datepicker-rtl .ui-datepicker-prev { right: 2px; left: auto; }
-.ui-datepicker-rtl .ui-datepicker-next { left: 2px; right: auto; }
-.ui-datepicker-rtl .ui-datepicker-prev:hover { right: 1px; left: auto; }
-.ui-datepicker-rtl .ui-datepicker-next:hover { left: 1px; right: auto; }
-.ui-datepicker-rtl .ui-datepicker-buttonpane { clear:right; }
-.ui-datepicker-rtl .ui-datepicker-buttonpane button { float: left; }
-.ui-datepicker-rtl .ui-datepicker-buttonpane button.ui-datepicker-current { float:right; }
-.ui-datepicker-rtl .ui-datepicker-group { float:right; }
-.ui-datepicker-rtl .ui-datepicker-group-last .ui-datepicker-header { border-right-width:0; border-left-width:1px; }
-.ui-datepicker-rtl .ui-datepicker-group-middle .ui-datepicker-header { border-right-width:0; border-left-width:1px; }
-
-/* IE6 IFRAME FIX (taken from datepicker 1.5.3 */
-.ui-datepicker-cover {
- display: none; /*sorry for IE5*/
- display/**/: block; /*sorry for IE5*/
- position: absolute; /*must have*/
- z-index: -1; /*must have*/
- filter: mask(); /*must have*/
- top: -4px; /*must have*/
- left: -4px; /*must have*/
- width: 200px; /*must have*/
- height: 200px; /*must have*/
-}/* Progressbar
-----------------------------------*/
-.ui-progressbar { height:2em; text-align: left; }
-.ui-progressbar .ui-progressbar-value {margin: -1px; height:100%; }
\ No newline at end of file
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/E.png b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/E.png
deleted file mode 100755
index c54150820a..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/E.png and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/N.png b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/N.png
deleted file mode 100755
index b7a747cd39..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/N.png and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/NE.png b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/NE.png
deleted file mode 100755
index 14d39ae0f0..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/NE.png and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/NW.png b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/NW.png
deleted file mode 100755
index e57382d0d4..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/NW.png and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/S.png b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/S.png
deleted file mode 100755
index edeabb494d..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/S.png and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/SE.png b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/SE.png
deleted file mode 100755
index e2e35ba552..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/SE.png and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/SW.png b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/SW.png
deleted file mode 100755
index 71f3be47ae..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/SW.png and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/W.png b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/W.png
deleted file mode 100755
index baa74b59ad..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/W.png and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/arrow.png b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/arrow.png
deleted file mode 100755
index 3a004f60b5..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/arrow.png and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/arrow2.png b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/arrow2.png
deleted file mode 100755
index d2cae73712..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/arrow2.png and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/blue-dot.png b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/blue-dot.png
deleted file mode 100755
index 98b280d301..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/blue-dot.png and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/favicon.ico b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/favicon.ico
deleted file mode 100755
index 7cd42a2755..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/favicon.ico and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/logo.png b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/logo.png
deleted file mode 100755
index 032f6d16e7..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/logo.png and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/losange.gif b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/losange.gif
deleted file mode 100755
index dfadf2d823..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/losange.gif and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/losange.png b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/losange.png
deleted file mode 100755
index 0786209c39..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/losange.png and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/red-dot.png b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/red-dot.png
deleted file mode 100755
index b0f3f0e928..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Icons/red-dot.png and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Interface.jsp b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Interface.jsp
deleted file mode 100755
index 3ef63cc678..0000000000
--- a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/Interface.jsp
+++ /dev/null
@@ -1,1943 +0,0 @@
-<%@page import="pow.webserver.Conf,java.util.StringTokenizer,java.util.NoSuchElementException" %>
-
-<%
- if (session.getAttribute("login")==null) {
- out.println("");
- }
-%>
-
-
-
-
-
-
-
- POW
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- ");
- }
- else {
- String login = ((String) (session.getAttribute("login"))).toString();
- String right = ((String) (session.getAttribute("rights"))).toString();
- if (right.equals("visitor"))
- { out.println("var usr_right=\"visitor\";");
- out.println("var usr_login=\"nologged\";");
- }
- else if (right.equals("admin"))
- {
- out.println("var usr_right=\"admin\";");
- out.println("var usr_login=\""+login+"\";");
- }
- else if (right.equals("user")){ // gestion de la liste des drones controlables
- out.println("var usr_right=\"user\";");
- out.println("var usr_login=\""+login+"\";");
- out.println("var drone_ctl=new Array();");
- String droneCtl = ((String) (session.getAttribute("dronectl"))).toString();
- StringTokenizer st = new StringTokenizer(droneCtl,";");
- String d;
- for(int i = 0 ; i< st.countTokens() ;i++){
- d=st.nextToken();
- out.println("drone_ctl[\""+ d +"\"]=1;");
- }
- try{
- while (st.hasMoreTokens()) {
- d=st.nextToken();
- }
- }
- catch (NoSuchElementException ex){}
-
- }
- }
- String default_folder = this.getServletConfig().getServletContext().getRealPath("");
- Conf myconf =new Conf(default_folder,"pow_conf.xml");
- out.println("//10sec after plane_die event remove drone");
- out.println("var dieEventTimeoutTime = "+myconf.getDieEventTimeoutTime()+";");
- out.println("//5sec without any position data about a drone remove drone");
- out.println("var dataEventTimeoutTime = "+myconf.getDataEventTimeoutTime()+";");
- out.println("// time to wait before order was cancelled");
- out.println("var order_response_timeout = "+myconf.getOrderResponseTimeout()+";");
- %>
- var noPlaneBefore=true; // pour initialiser au premier push flight param
- var ajax_url = "ajaxRqst.srv";
-/* ********** drone state timeout ******** */
- var droneStateDieEvent = new Array(); // timeout en cas d'event die
- var droneStateDataEvent = new Array(); // timeout en cas de non evenement data
- var waypoint_modif = new Array(); //
- var block_jump_timeout=-1;
- var setting_change_timeout=-1;
- var setting_to_change_id=-1;
-/* *************************************************************************** Global variables *************************************************************************************** */
- var map;
-
- var planes = new Array();
- var nb_planes =0;
- var markers = new Array();
- var markers_color = new Array(); // stock les balises de couleurs de chaque drone
- var diametre_drone_balise = 50; //50m de base
- var selected_plane = "";
- var selected_plane_id = 0;
- var selected_index=0;
- var tracking = false;
-
-/* *******************Flight Plan variables :******************** */
- var waypoints = new Array();
- var waypoint_tmp = null;
- var index_wpt = 0;
- var blocks= new Array();
- var index_block = 0;
- var active_block_id=0;
- var active_block_name="";
- var fpl_name="";
- var lat0=0;
- var lon0=0;
- var fpl_alt=0;
- var security_height=0;
- var fpl_ground_alt=0;
- var max_dist=0;
-
-
-/* ********************Planes and waypoints icons******************** */
- var plane_icons = new Array();
- for (var i=0;i<8;i++){
- plane_icons[i] = new GIcon();
- plane_icons[i].shadow = "http://labs.google.com/ridefinder/images/mm_20_shadow.png";
- plane_icons[i].iconSize = new GSize(60,60);
- plane_icons[i].shadowSize = new GSize(22, 20);
- plane_icons[i].infoWindowAnchor = new GPoint(50, 1);
- plane_icons[i].iconAnchor = new GPoint(30, 30);
- }
- plane_icons[0].image="Icons/N.png";
- plane_icons[1].image="Icons/NE.png";
- plane_icons[2].image="Icons/E.png";
- plane_icons[3].image="Icons/SE.png";
- plane_icons[4].image="Icons/S.png";
- plane_icons[5].image="Icons/SW.png";
- plane_icons[6].image="Icons/W.png";
- plane_icons[7].image="Icons/NW.png";
-
- var wpt_icon=new GIcon();
- wpt_icon.shadow = "http://labs.google.com/ridefinder/images/mm_20_shadow.png";
- wpt_icon.iconSize = new GSize(20,20);
- wpt_icon.shadowSize = new GSize(22, 20);
- wpt_icon.infoWindowAnchor = new GPoint(8, 1);
- wpt_icon.iconAnchor = new GPoint(6, 20);
- //wpt_icon.image="Icons/losange.png";
- wpt_icon.image="Icons/blue-dot.png";
-
- var iconMarkerTemp=new GIcon();
- iconMarkerTemp.shadow = "http://labs.google.com/ridefinder/images/mm_20_shadow.png";
- iconMarkerTemp.iconSize = new GSize(20,20);
- iconMarkerTemp.shadowSize = new GSize(22, 20);
- iconMarkerTemp.infoWindowAnchor = new GPoint(8, 1);
- iconMarkerTemp.iconAnchor = new GPoint(6, 20);
- iconMarkerTemp.image="Icons/red-dot.png";
-/* *************************************************************************** Functions **************************************************************************************************** */
-
-/* ************ prevent user from going back to this page by browser's forward button****************************** */
-
-function backButtonOverride()
-{
- // Work around a Safari bug
- // that sometimes produces a blank page
- setTimeout("backButtonOverrideBody()", 1);
-
-}
-
-function backButtonOverrideBody()
-{
- // Works if we backed up to get here
- try {
- history.forward();
- } catch (e) {
- // OK to ignore
- }
- // Every quarter-second, try again. The only
- // guaranteed method for Opera, Firefox,
- // and Safari, which don't always call
- // onLoad but *do* resume any timers when
- // returning to a page
- setTimeout("backButtonOverrideBody()", 500);
-}
-/* ******************* Useful functions ******************** */
-// Gets an element of the html document by its class name
- function getElementsByClass(tag, className){
- var elements = document.getElementsByTagName(tag);
- var results = new Array();
- for(var i=0; i< elements.length; i++){
- if(elements[i].className == className){
- results[results.length] = elements[i];
- }
- }
- return results;
- }
-
-
- function pause(time){
- d=new Date();
- diff=0;
- while(diff < time){
- n=new Date();
- diff=n-d;
- }
- }
-
-
- //Creates a GoogleMaps marker
-
-
-
-
- function createMarker(point, legend, icon){
- var marker = new GMarker(point, icon);
- GEvent.addListener(marker, 'click', function() {
- marker.openInfoWindowHtml(legend);
- });
- return marker;
- }
-
- var old_lat;
- var old_lon;
- //Creates a draggable marker with an EventListener
- function createDraggableMarker(point,namewpt,wpt_icon,index_wpt, bool){
- var fpl_file_name="upload/"+planes[selected_index]["id"]+"/flight_plan.xml";
- var marker = new GMarker(point,{legend: namewpt,icon: wpt_icon,draggable: bool,bouncy:true});
- //var marker = new GMarker(point,{title: name,draggable: bool});
- //var marker = new GMarker(point,{draggable: bool});
- GEvent.addListener(marker, 'click', function() {
- marker.openInfoWindowHtml(namewpt);
- });
- if (bool){
- GEvent.addListener(marker, 'dragend', function(latlng) {
- if (latlng){
- //alert("id wpt="+index_wpt+" name=" +namewpt+" lat="+latlng.lat()+" lon="+latlng.lng());
- var lat = latlng.lat();
- var lon = latlng.lng();
- moveWpt(selected_plane_id,namewpt,lat,lon,false,true,index_wpt);
- }
- });
-
- GEvent.addListener(marker, 'dragstart', function(latlng) {
- if (latlng){
- old_lat=latlng.lat();
- old_lon=latlng.lng();
- }
- });
- }
-
- return marker;
- }
-
-
-
-
-
-//Converts ditances from (lat0,lon0) given in meters into latitudes or longitudes, and vice versa
- function xMetersToDegrees(x,lat0){
- return (x/1852/60+parseFloat(lat0));
- }
- function yMetersToDegrees(y,lat,lon0){
- return (y/1852/60*Math.cos(lat)+parseFloat(lon0));
- }
-
- function xDegreesToMeters(lat,lat0){
- return((lat-lat0)*1852*60);
- }
- function yDegreesToMeters(lat,lon,lon0){
- return(lat!=0?(lon-lon0)/Math.cos(lat)*1852*60:0);
- }
-
-
- function name_from_id(id){
- var match=false;
- for (var i=0;i< planes.length;i++){
- if(planes[i]["id"]==id){match=true;break;}
- }
- if (match){
- return (planes[i]["name"]);
- }else{
- return (null);
- }
- }
- /* *************** JQUERY stuffs ***************************** */
- $(function(){
- //init();
- //$("#Settings").draggable();$("#Settings").resizable();
- //$("#map_canvas").draggable();$("#map_canvas").resizable();
- //$("#Debug").draggable();$("#Debug").resizable();
- //$("#choicePanel").draggable();$("#choicePanel").resizable();
- //$("#FlightPlan").draggable();$("#FlightPlan").resizable();
- //$("#FlightParams").draggable();$("#FlightParams").resizable();
- });
-/* ********************* Fonctions de Push ******************** */
-
- function initialize_push() {
- // alert('go');
- p_join_listen(null, 'stream');
- // TODO il faudra gerer le cas ou cela bloque !!!! et passer en pull
- // p_join_listen(null, 'pull');
- p_subscribe('/data/drone/iskill', 'msg from serveur');
- p_subscribe('/data/drones_maj', 'msg from serveur');
- p_subscribe('/data/order/waypoint_moved', 'msg from serveur');
- p_subscribe('/data/order/change_setting', 'msg from serveur');
- p_subscribe('/data/order/plane_die', 'msg from serveur');
- p_subscribe('/data/order/plane_resurect', 'msg from serveur');
- p_subscribe('/data/order/new_plane', 'msg from serveur');
- p_subscribe('/data/order/block_changed', 'msg from serveur');
- p_subscribe('/data/order/other', 'msg from serveur');
- p_subscribe('/data/order/settings', 'msg from serveur');
-
- p_subscribe('/chat','msg from web client');
- p_subscribe('/client_action','action from web client');
- }
-
- function initialize_push_pullmode() {
-
- p_join_listen(null, 'pull');
- p_subscribe('/data/drone/iskill', 'msg from serveur');
- p_subscribe('/data/drones_maj', 'msg from serveur');
- p_subscribe('/data/order/waypoint_moved', 'msg from serveur');
- p_subscribe('/data/order/change_setting', 'msg from serveur');
- p_subscribe('/data/order/plane_die', 'msg from serveur');
- p_subscribe('/data/order/plane_resurect', 'msg from serveur');
- p_subscribe('/data/order/new_plane', 'msg from serveur');
- p_subscribe('/data/order/block_changed', 'msg from serveur');
- p_subscribe('/data/order/other', 'msg from serveur');
- p_subscribe('/data/order/settings', 'msg from serveur');
-
- p_subscribe('/chat','msg from web client');
- p_subscribe('/client_action','action from web client');
- }
-
- function displayControl(aString) {
- document.debugEventDisplay.event.value = aString;
- }
- // callback on data Events
- // call apropirate function according to the received event
-
- function onData(event) {
- var subject = event.get('p_subject');
-
- // displayControl(""+ event.toString());
-
- if (subject=="/data/drones_maj") { handleDronePositionUpsate(event);}
- else if (subject =="/data/order/waypoint_moved") {orderprocessing_waypoint(event);}
- else if (subject =="/data/order/block_changed") {orderprocessing_jump2block(event);}
- else if (subject =="/data/order/change_setting") {orderprocessing_setting(event);}
- else if (subject =="/data/order/plane_die") {orderprocessing_planedie(event);}
- else if (subject =="/data/order/plane_resurect"){orderprocessing_planeressurect(event);}
- else if (subject =="/data/order/new_plane") {orderprocessing_newplane(event);}
- else if (subject =="/data/drone/iskill") {orderprocessing_planekilled(event);}
- else if (subject =="/data/order/settings") {orderprocessing_csv_settings(event);}
- else if (subject =="/chat") {orderprocessing_chat(event);}
- else if (subject =="/client_action") {orderprocessing_client_action(event);}
- }
-
- // callback on all other kind of Events
-
- <%--function onEvent(event) {
- var subject = event.get('p_subject');
- displayControl("EVENT CALLBACK "+subject+"\n" + event.toString());
-
-
- }
- --%>
-/* ********************* initilization de l'interface ******************* */
- function initialize_display(){
- var aircraftList = document.getElementById("aircraftForm").aircraftList;
- if (nb_planes ==0)//If there is no active aircraft, the list only has one option : No aircraft
- {aircraftList.options[0]=new Option("No aircraft");}
-}
-
-/* *********************functions to check airplanes ******************* */
-
-/* *********************Processing events ******************* */
- // recherche si l'id de l'avion est deja present dans le tableau des drones deja
- function handleDronePositionUpsate(event){
- if (planes.length==0) {noPlaneBefore=true;}
- var id = parseInt(event.get('aircraftId'));
- var match=false;
- var insert=false;
- var aircraftList = document.getElementById("aircraftForm").aircraftList;
- nb_planes=planes.length;
- for (var i=0;i< nb_planes;i++){
- if(planes[i]["id"]> id){insert=true;break;} // on garde le tableau trié
- if (planes[i]["id"]==id){match=true;break;} // on recupere en i le drone correspondant à id
- }
- if(!match){
- var k=0;
- if(insert){ // on insert au milieu
- k=i;
- planes.splice(i,0,new Array());
- markers.splice(i,0,null);
- markers_color.splice(i,0,null);
- // on insere l'aircraft dan sla liste deroulante
- //see http://www.mredkj.com/tutorials/tutorial005.html
- var elOptNew = document.createElement('option');
- elOptNew.text = event.get('dbName')
- var elOptOld = aircraftList.options[i];
- try {
- aircraftList.add(elOptNew, elOptOld); // standards compliant; doesn't work in IE
- }
- catch(ex) {
- aircraftList.add(elOptNew, i); // IE only
- }
- }else{ // on met à la fin du tableau
- k=planes.length;
- planes[k] = new Array();
- markers[k]= null; // inutile
- markers_color[k]=null;
- aircraftList.options[k]=new Option(event.get('dbName'));
- }
- droneStateDieEvent[k] = -1; // init un timeout vide
- //
- planes[k]["id"]=id;
- planes[k]["name"] =event.get('dbName');
- planes[k]["lat"] = event.get('dbLatitude');
- planes[k]["lon"] = event.get('dbLongitude');
- planes[k]["heading"]= event.get('dbCourse');
- // maj des param pour selected acrf
- planes[k]["speed"]= event.get('dbSpeed');
- planes[k]["altitude"]= event.get('dbAmsl');
- planes[k]["vspeed"]= event.get('dbVert_speed');
- planes[k]["height"]= event.get('dbAgl');
- planes[k]["battery"]= event.get('dbStat_battery');
- planes[k]["GPS"]= event.get('dbStat_gps');
- planes[k]["activeBlock"]= event.get('dbActive_block');
- planes[k]["engine"]= event.get('dbEngine_status');
- planes[k]["setting_id"]= event.get('dbId_Setting');
- planes[k]["setting_value"]= event.get('dbSetting_Value');
- planes[k]["drone_color"]= event.get('drone_color');
- // gere si le drone peut etre controlé ou non
- if ((usr_right=="admin")||((usr_right=="user")&&(drone_ctl[event.get('dbName')]!=null))){
- planes[k]["rights"]= 1;
- }
- else
- {planes[k]["rights"]=0;}
-
- var icon;
- if (planes[k]["heading"]>=338 || planes[i]["heading"]< 23){
- icon = plane_icons[0];
- }else if (planes[k]["heading"]>=23 && planes[i]["heading"]<68){
- icon = plane_icons[1];
- }else if (planes[k]["heading"]>=68 && planes[i]["heading"]<113){
- icon = plane_icons[2];
- }else if (planes[k]["heading"]>=113 && planes[i]["heading"]<158){
- icon = plane_icons[3];
- }else if (planes[k]["heading"]>=158 && planes[i]["heading"]<203){
- icon = plane_icons[4];
- }else if (planes[k]["heading"]>=203 && planes[i]["heading"]<248){
- icon = plane_icons[5];
- }else if (planes[k]["heading"]>=248 && planes[i]["heading"]<293){
- icon = plane_icons[6];
- }else if (planes[k]["heading"]>=293 && planes[i]["heading"]<337){
- icon = plane_icons[7];
- }
- //A marker is created and added on the map
- var pos = new GLatLng(planes[k]["lat"],planes[k]["lon"]);
- markers[k]=createMarker(pos,planes[k]["name"],icon);
- markers_color[k] = GPolygon.Circle(pos,diametre_drone_balise,"#000000",1,1,planes[i]["drone_color"],0.5)
- map.addOverlay(markers[k]);
- map.addOverlay(markers_color[k]);
- droneStateDataEvent[k] = setTimeout("nodataEventCallBack("+id+")",dataEventTimeoutTime);
- }
- else // l event envoyé correspond à un drone deja dans le tableau
- {
- if (droneStateDataEvent[i]!=-1) {clearTimeout(droneStateDataEvent[i]);}
- var heading_changed;
- //Update of the planes array
- planes[i]["lat"] = event.get('dbLatitude');
- planes[i]["lon"] = event.get('dbLongitude');
- // maj des param pour selected acrf
- planes[i]["speed"]= event.get('dbSpeed');
- planes[i]["altitude"]= event.get('dbAmsl');
- planes[i]["vspeed"]= event.get('dbVert_speed');
- planes[i]["height"]= event.get('dbAgl');
- planes[i]["battery"]= event.get('dbStat_battery');
- planes[i]["GPS"]= event.get('dbStat_gps');
- planes[i]["activeBlock"]= event.get('dbActive_block');
- planes[i]["engine"]= event.get('dbEngine_status');
- planes[i]["setting_id"]= event.get('dbId_Setting');
- planes[i]["setting_value"]= event.get('dbSetting_Value');
- planes[i]["drone_color"]= event.get('drone_color');
- //
- var new_heading = event.get('dbCourse');
- var old_heading = planes[i]["heading"];
- //Update of the icons if the heading has changed
- if (Math.abs(new_heading-old_heading)>2){
- var image;
- if (new_heading>=338 || new_heading< 23){
- image = plane_icons[0].image;
- }else if (new_heading>=23 && new_heading<68){
- image = plane_icons[1].image;
- }else if (new_heading>=68 && new_heading<113){
- image = plane_icons[2].image;
- }else if (new_heading>=113 && new_heading<158){
- image = plane_icons[3].image;
- }else if (new_heading>=158 && new_heading<203){
- image = plane_icons[4].image;
- }else if (new_heading>=203 && new_heading<248){
- image = plane_icons[5].image;
- }else if (new_heading>=248 && new_heading<293){
- image = plane_icons[6].image;
- }else if (new_heading>=293 && new_heading<337){
- image = plane_icons[7].image;
- }
- markers[i].setImage(image);
- }
- planes[i]["heading"] = new_heading;
- //Setting the marker to its new position
- var new_pos = new GLatLng(planes[i]["lat"],planes[i]["lon"]);
- markers[i].setLatLng(new_pos);
- //see http://econym.org.uk/gmap/eshapes.htm
- map.removeOverlay(markers_color[i]);
- markers_color[i] = GPolygon.Circle(new_pos,diametre_drone_balise,"#000000",1,1,planes[i]["drone_color"],0.5);
- map.addOverlay(markers_color[i]);
- droneStateDataEvent[i] = setTimeout("nodataEventCallBack("+id+")",dataEventTimeoutTime);
- }
- nb_planes=planes.length;
-
- document.getElementById("aircraftList").options.length=planes.length;
- if (tracking&&nb_planes!=0){//If tracking is activated, the map is centered on the aircraft
- var zoom = map.getZoom();
- var plane_number = aircraftList.selectedIndex;
- map.setCenter(new GLatLng(planes[plane_number]["lat"],planes[plane_number]["lon"]),zoom);
- }
-
- if (noPlaneBefore==true){
- noPlaneBefore=false;// !!! interruption possible
- active_block_id=planes[0]["activeBlock"];
- aircraftList.options[0].selected=true;
- selected_plane = planes[0]["name"];
- selected_plane_id = planes[0]["id"];
- DOMImplementation("upload/"+planes[0]["id"]+"/flight_plan.xml",fplDisplay);
- DOMImplementation("upload/"+planes[0]["id"]+"/settings.xml",settingsDisplay);
- }
- selected_plane_update(selected_plane_id);
- }
-
-
-/* ******************** affichage log ******************* */
-
-var coloryellow = true;
-function addMsgLog(msg)
-{
- var element = document.getElementById("info");
- var newinfodiv = document.createElement("div");
- var newinfotxt = document.createTextNode(msg);
- if (coloryellow) {newinfodiv.style.background = '#FFE4B5';coloryellow=false;}
- else {newinfodiv.style.background = '#D2B48C';coloryellow=true;}
- newinfodiv.appendChild(newinfotxt);
- element.appendChild(newinfodiv);
-}
-/* ******************** Initialization of the page ******************* */
-
-/* Initialization of the page : map, markers, aircraft list... */
-
-
- function initialize() {
-
- if (GBrowserIsCompatible()) {
- // initialize_planes_data();
- initialize_display();
- initialize_push();
- map = new GMap2(document.getElementById("map_canvas"));//Map creation
- map.setMapType(G_HYBRID_MAP);
- var point= new GLatLng(43.46223, 1.27289);
- map.setCenter(point,15);//Center the map on a temporary point
- map.setUIToDefault();
- //
- GEvent.addListener(map, "zoomend", function(oldlevel,newlevel) {
- var d = newlevel-oldlevel;
- if (d<0) {
- diametre_drone_balise=diametre_drone_balise*(-2*d);
- }
- else
- {
- diametre_drone_balise=diametre_drone_balise/(2*d);
- }
- });
- }else{
- alert("Your browser is not compatible with Google Maps !");
- }
- }
-
-
-
-/* Updating of the selected aircraft flight parameters */
-// recherche la place de l'avion dans le tableau
- function seekIndex(plane_id)
- {
- var i = 0;
- var trouve = false;
- var res=-1;
- while ((ilat y->lon !');
- x = parseFloat(childXML.getAttribute("x"));
- y = parseFloat(childXML.getAttribute("y"));
- lat=xMetersToDegrees(x,lat0);
- lon=yMetersToDegrees(y,lat,lon0);
- }
- else
- {
- lat=parseFloat(childXML.getAttribute("lat"));
- lon=parseFloat(childXML.getAttribute("long"));
- }
- var right=planes[selected_index]["rights"];
- waypoints[index_wpt]["marker"]=null;
-
- if (right==1){
- waypoints[index_wpt]["marker"]=createDraggableMarker(new GLatLng(lat,lon),nameWpt,wpt_icon,index_wpt, true);
- }else {
- waypoints[index_wpt]["marker"]=createDraggableMarker(new GLatLng(lat,lon),nameWpt,wpt_icon,index_wpt, false);
- }
- map.addOverlay(waypoints[index_wpt]["marker"]);
-
- var alt=false;
- if (childXML.getAttribute("alt")!=null){
- alt=childXML.getAttribute("alt");
- waypoints[index_wpt]["alt"]=parseFloat(childXML.getAttribute("alt"));
- }
- var node_url="";
- if (right==1){
- node_url = "javascript:moveWpt("+selected_index+",'"+nameWpt+"',"+lat+","+lon+","+alt+",false,"+(index_wpt)+")";
- }
- /////
- index_wpt++;
- var newindex = index++;
- fpl_tree.add(newindex,index_pere,nameWpt,node_url);
- for(var k =0;k< nbr_attrib;k++){
- var newindex_attrib_name = index++;
- var newindex_attrib_value = index++;
- fpl_tree.add(newindex_attrib_name,newindex,childXML.attributes[k].nodeName);
- fpl_tree.add(newindex_attrib_value,newindex_attrib_name,childXML.attributes[k].nodeValue);
- //txt = txt + childXML.attributes[k].nodeName + "=" + childXML.attributes[k].nodeValue +" ";
- }
- }
- }
- else if (childXML.nodeName=="block")
- { var node_url="";var txt_block_name="";
- if(childXML.attributes!=null){
- var block_name=childXML.getAttribute("name");//.replace(/ /g, '_');
- blocks[index_block]=new Array();
- blocks[index_block]["name"]=block_name;
- if (active_block_id==index_block){
- active_block_name=block_name;
- txt_block_name = ""+block_name+" ";
- }
- else
- {
- txt_block_name = block_name;
- }
- var right=planes[selected_index]["rights"];
-
- if (right==1){
- node_url="javascript:activateBlock("+selected_index+",'"+block_name+"',"+index_block+")";
- }
- index_block++;
- var txt = txt_block_name+ " : ";
- var nbr_attrib = childXML.attributes.length;
- for(var k =0;k< nbr_attrib;k++){
- if (childXML.attributes[k].nodeName!="name"){
- var newindex_attrib_name = index++;
- var newindex_attrib_value = index++;
- txt = txt + childXML.attributes[k].nodeName + "=" + childXML.attributes[k].nodeValue +" ";
- }
- }
- }
- var newindex = index++;
- fpl_tree.add(newindex,index_pere,txt,node_url);
- }
- // recursion
- if(childXML.hasChildNodes()) {
- var nodes=childXML.childNodes.length;
- for(var i=0; i90.0)||(l<-90.0)||isNaN(new_lat)) {
- alert('wrong format for a latitude... should be within [-90:+90]');
- return;
- }
- new_lon=prompt("longitude=",mylon);
- if ((new_lon==null)){
- return;
- }
- l = parseFloat(new_lon);
- if ((l>180.0)||(l<-180.0)||isNaN(new_lon)) {
- alert('wrong format for a longitude... should be within [-180:+180]');
- return;
- }
- if (alt){
- new_alt=prompt("altitude=",alt);
- if ((new_alt==null)){
- return;
- }
- l = parseFloat(new_alt);
- if ((l<=0.0)||isNaN(new_alt)) {
- alert('wrong format for an altitude... should be positive');
- return;
- }
- }
- //
- old_lat = waypoints[idwpt]["lat"];
- old_lon = waypoints[idwpt]["lon"];
- }else{
- new_lat=mylat;
- new_lon=mylon;
- }
-
-
- //Ajax request to edit the flight plan xml file
- var xhr = getXMLHttpRequest();
- xhr.onreadystatechange = function() {
- if (xhr.readyState == 4 && xhr.status == 200) {
- // maj de l'affichage fpl
- var xmlResponse = xhr.responseXML.documentElement;
- var rep = xmlResponse.getElementsByTagName("waypoint_to_move")[0];
- var idwpt_requested = parseInt(rep.getAttribute("idwpt"))-1;// cf waypoint fictif ivy
- var acid_requested = parseInt(rep.getAttribute("acid"));
- var lat_requested = parseFloat(rep.getAttribute("newlat"));
- var lon_requested = parseFloat(rep.getAttribute("newlon"));
- var was_dragged = rep.getAttribute("dragged");
- var name_drone = planes[seekIndex(acid_requested)]["name"];
- // on ne met à jour visuellement que si le focus est sur le drone selecté
- if (acid_requested==selected_plane_id){
- // mettre le waypoint dans un etat intermediaire
- // en attendant l'accuse de reception
- var right=planes[selected_index]["rights"];
- var marker_tmp;
- if (right==1){
- marker_tmp=createDraggableMarker(new GLatLng(old_lat,old_lon),waypoints[idwpt_requested]["name"],wpt_icon,idwpt_requested, true);
- }else {
- marker_tmp=createDraggableMarker(new GLatLng(old_lat,old_lon),waypoints[idwpt_requested]["name"],wpt_icon,idwpt_requested, false);
- }
- var name_wpt = waypoints[idwpt_requested]["name"];
- var timer=setTimeout("callback_waypoint("+acid_requested+","+idwpt_requested+")",order_response_timeout);
- waypoint_modif[idwpt_requested] = new createWaypoint(acid_requested,idwpt_requested,name_wpt,marker_tmp,timer);
- // on efface le marker courant
- // meme si elle a ete deplacé par drag & drop
- // on cree un marker non draggable avec une icone temporaire
- map.removeOverlay(waypoints[idwpt_requested]["marker"]);
- waypoints[idwpt_requested]["marker"] = createMarker(new GLatLng(lat_requested,lon_requested),name_wpt,iconMarkerTemp);
- if (waypoints[idwpt_requested]["marker"]!=null) map.addOverlay(waypoints[idwpt_requested]["marker"]);
- else alert('echec creation wpt tmp');
- addMsgLog("waypoint "+ name_wpt +" for drone " + name_drone +" will be moved");
- }
- else
- {
- addMsgLog("waypoint "+ idwpt_requested +" for drone " + name_drone +" will be moved");
- }
-
- }
- };
- xhr.open("POST",ajax_url,true);
- xhr.setRequestHeader("Content-Type", "application/x-www-form-urlencoded");
- // on informe les autres clients de ne pas modifier le waypoint en cours (via pushlet)
- p_publish('/client_action', 'login',usr_login,'action', 'lock', 'type_obj', 'waypoint', 'id_obj',idwpt,'webiddrone',selected_plane_id);
- // on passe en parametre le index_wpt+1 car dans paparazzi il y a un wpt predefini a zero
-
- if(alt!=false){
- xhr.send("order=fpl_update&wpt_name="+name+"&aircraft_id=" +selected_plane_id+"&wpt_id="+(idwpt+1)+"&new_lat="+new_lat+"&new_lon="+new_lon+"&new_alt="+new_alt+"&dragged="+dragged+"&new_alt_for_fpl=1");
- //alert("wptid="+i+" lat="+new_lat+"lon="+new_lon+"alt="+new_alt);
- }else{
- var alti=document.getElementById("altitude");var h=parseFloat(alti.innerHTML);
- xhr.send("order=fpl_update&wpt_name="+name+"&aircraft_id=" +selected_plane_id+"&wpt_id="+(idwpt+1)+"&new_lat="+new_lat+"&new_lon="+new_lon+"&new_alt="+parseFloat(alti.innerHTML)+"&dragged="+dragged+"&new_alt_for_fpl=0");
- //alert("wptid="+i+" lat="+new_lat+"lon="+new_lon+"alt="+h);
- }
-}
-
-
-/* ******** Activate a flight plan block ******** */
-
-
-
- function activateBlock(ac_webid,blockname,block_id){
- // verifie qu'il n'y pas pas un verrou sur le block posé par un autre client
- var k=0;
- var trouve = false;
- while ((!trouve)&&(k= parseFloat(min)) ){
- correct=true;
- var xhr = getXMLHttpRequest();
- xhr.onreadystatechange = function() {
- if (xhr.readyState == 4 && xhr.status == 200) {
- var xmlResponse = xhr.responseXML.documentElement;
- var rep = xmlResponse.getElementsByTagName("setting_to_change")[0];
- var idsetting_requested = parseInt(rep.getAttribute("setting_id"));
- var acid_requested = parseInt(rep.getAttribute("acid"));
- var name_drone = planes[seekIndex(acid_requested)]["name"];
- addMsgLog("request change setting "+ idsetting_requested +" for drone " + name_drone +" has been sent");
- // mise en place d'un timer d'accusé reception
- if(acid_requested==planes[selected_index]["id"]){
- setting_change_timeout=setTimeout("callback_setting("+acid_requested+","+idsetting_requested+")",order_response_timeout);
- setting_to_change_id = idsetting_requested;
- }
- }
- };
- xhr.open("POST",ajax_url,true);
- xhr.setRequestHeader("Content-Type", "application/x-www-form-urlencoded");
- //Send the parameters to the php page :
- xhr.send("order=modif_setting&aircraft_id=" +selected_plane_id+"&setting_id="+idsetting+"&value="+new_value);
-
- }else{
- new_value=prompt("Wrong value, please enter a value between "+min+" and "+max+" with a step of "+step+" :");
- if (new_value==null){return;}
- }
- }
- }
-
- function changeSetting2(idsetting,new_value){
-
- var xhr = getXMLHttpRequest();
- xhr.onreadystatechange = function() {
- if (xhr.readyState == 4 && xhr.status == 200) {
- var xmlResponse = xhr.responseXML.documentElement;
- var rep = xmlResponse.getElementsByTagName("setting_to_change")[0];
- var idsetting_requested = parseInt(rep.getAttribute("setting_id"));
- var acid_requested = parseInt(rep.getAttribute("acid"));
- var name_drone = planes[seekIndex(acid_requested)]["name"];
- addMsgLog("request change setting "+ idsetting_requested +" for drone " + name_drone +" has been sent");
- // mise en place d'un timer d'accusé reception
- if(acid_requested==planes[selected_index]["id"]){
- setting_change_timeout=setTimeout("callback_setting("+acid_requested+","+idsetting_requested+")",order_response_timeout);
- setting_to_change_id = idsetting_requested;
- }
- }
- };
- // on informe les autres clients de ne pas modifier le setting en cours (via pushlet)
- p_publish('/client_action', 'login',usr_login,'action', 'lock', 'type_obj', 'setting', 'id_obj',idsetting,'webiddrone',selected_plane_id);
- //
- xhr.open("POST",ajax_url,true);
- xhr.setRequestHeader("Content-Type", "application/x-www-form-urlencoded");
- //Send the parameters to the servlet :
- xhr.send("order=modif_setting&aircraft_id=" +selected_plane_id+"&setting_id="+idsetting+"&value="+new_value);
-
- }
-
-/* ************************* handle event orders from server ********************************************* */
-
- function orderprocessing_waypoint(event){
-
- var acft_id = parseInt(event.get('aircraftId'));
- var wpt_id = parseInt(event.get('waypointId'))-1; // cf indentation des waypoint sous ivy
- var lat = event.get('latitude');
- var lon = event.get('longitude');
- var alt = event.get('altitude');
- var name=name_from_id(acft_id);
- if ((wpt_id>=0)&&(acft_id==selected_plane_id)) {
- if (waypoint_modif[wpt_id]!=null)
- {
- addMsgLog('waypoint ' + wpt_id+' for drone '+name+ ' has been moved to '+lat+"//"+lon+' !');
- clearTimeout(waypoint_modif[wpt_id].timer)
- waypoint_modif[wpt_id] = null;
- DOMImplementation("upload/"+planes[selected_index]["id"]+"/flight_plan.xml",fplDisplay);
- // inform other web client that waypoint can be changed
- p_publish('/client_action', 'login',usr_login,'action', 'unlock', 'type_obj', 'waypoint', 'id_obj',wpt_id,'webiddrone',acft_id);
- }
- }
- //document.getElementById("info").innerHTML='waypoint' + wpt_id+' for drone '+name+ ' has been moved !';
- //addMsgLog('waypoint' + wpt_id+' for drone '+name+ ' has been moved !');
- //alert(' waypoint' + wpt_id+' for drone '+name+ ' has been moved !');
- }
-
- function orderprocessing_jump2block(event) {
- var acft_id = event.get('aircraftId');
- var current_block_id = event.get('currentBlockId');
- if (acft_id==selected_plane_id){
- /// maj de l'affichage fpl
- var name_block=blocks[current_block_id]["name"];
- var name_drone=name_from_id(acft_id);
- clearTimeout(block_jump_timeout);
- active_block_id = current_block_id;
- active_block_name =name_block;
- // inform other web client that block can be changed
- p_publish('/client_action', 'login',usr_login,'action', 'unlock', 'type_obj', 'block', 'id_obj',current_block_id,'webiddrone',acft_id);
- addMsgLog('drone '+ name_drone + ' has jump to block '+ name_block +' !');
- DOMImplementation("upload/"+planes[selected_index]["id"]+"/flight_plan.xml",fplDisplay);
- }
- }
-
- function orderprocessing_setting(event) {
- var acft_id = event.get('aircraftId');
- var setting_id = parseInt(event.get('settingId'));
- var value = event.get('settingValue');
- if ((acft_id==selected_plane_id)&&(setting_to_change_id==setting_id)){
- clearTimeout(setting_change_timeout);
- setting_to_change_id = -1; // reset
- var name=name_from_id(acft_id);
- //DOMImplementation("upload/"+planes[selected_index]["id"]+"/settings.xml",settingsDisplay);
- selected_plane_update(acft_id);
- addMsgLog('setting ' + setting_id +' for drone '+ name + ' has been changed to value '+ value +' !');
- // inform other web client that block can be changed
- p_publish('/client_action', 'login',usr_login,'action', 'unlock', 'type_obj', 'setting', 'id_obj',setting_id,'webiddrone',acft_id);
- }
- }
-
-
- function orderprocessing_planedie(event){
- var acft_id = event.get('aircraftId');
- var order_string = event.get('order');
- var name=name_from_id(acft_id);
- //document.getElementById('info').innerHTML='drone ' + name+' is not alive !';
- addMsgLog('drone ' + name+' is not alive !');
- var drone_index = seekIndex(acft_id);
- if(drone_index!=-1)
- {
- addMsgLog('drone ' + name+' will be removed in 10 seconds !');
- droneStateDieEvent[drone_index] = setTimeout("droneDieCallback("+acft_id+")", dieEventTimeoutTime);
- // on grise l'aircraft sur l'interface
- }
- }
-
- function orderprocessing_planeressurect(event){
- var acft_id = event.get('aircraftId');
- var order_string = event.get('order');
- //document.getElementById('info').innerHTML='drone ' +name+' has been resurrected !';
- addMsgLog('drone ' +acft_id+' has been resurrected !');
- var drone_index = seekIndex(acft_id);
- if(drone_index!=-1)
- {
- clearTimeout ( droneStateDieEvent[drone_index] );
- // on remet l'aircraft en couleur
- }
- }
-
- function orderprocessing_newplane(event){
- var acft_id = event.get('aircraftId');
- var order_string = event.get('order');
- var name= event.get('aircraftName');
- //document.getElementById('info').innerHTML='drone ' +name+' has been connected !';
- addMsgLog('drone ' +name+' has been connected !');
- }
-
-
- function orderprocessing_planekilled(event){
- var acft_id = event.get('aircraftId');
- var name=name_from_id(acft_id);
- addMsgLog("drone " + name + " deconnection caused by death of ivy bus...");
- //document.getElementById('info').innerHTML="drone " + name + " deconnection caused by death of ivy bus...";
- // remove drone...markers....
- removeDroneDisplay(acft_id);
- }
-
- function orderprocessing_csv_settings(event){
- var acft_id = event.get('aircraftId');
- var csv = event.get('csv');
- var index_drone = seekIndex(acft_id);
- var name=name_from_id(acft_id);
- if (index_drone!=-1) {
- planes[index_drone]["csv"]= csv;
- //addMsgLog("settings updated for drone " + name);
- if(index_drone==selected_index)
- {
- DOMImplementation("upload/"+planes[selected_index]["id"]+"/settings.xml",settingsDisplay);
- }
- }
- }
-
- function orderprocessing_chat(event){
- }
-
- function createLock(id_drone,typ_obj,id_obj,login){
- this.id_drone = id_drone;
- this.type_object = typ_obj;
- this.id_object = id_obj;
- this.request_login = login;
- this.lockmsg = function()
- {return ("user "+this.request_login+ " has locked "+this.type_object +" " +this.id_object);}
- this.unlockmsg = function()
- {return ("user "+this.request_login+ " has unlocked "+this.type_object +" " +this.id_object);}
- }
-
- var external_waypoint_lock = new Array();
- var external_block_lock = new Array();
- var external_setting_lock = new Array();
- // set or remove a lock to prevent user to change sthg taht is to be changed by another user
- function orderprocessing_client_action(event){
- var rqst_login = event.get('login');
- var rqst_action = event.get('action');
- var rqst_obj_type = event.get('type_obj');
- var rqst_obj_id = event.get('id_obj');
- var rqst_id_drone = event.get('webiddrone');
- if (rqst_action=='lock'){
- var lock_obj = new createLock(rqst_id_drone,rqst_obj_type,rqst_obj_id,rqst_login);
- if (rqst_obj_type=='waypoint') {external_waypoint_lock.push(lock_obj);}
- else if (rqst_obj_type=='block') {external_block_lock.push(lock_obj);}
- else if (rqst_obj_type=='setting') {external_setting_lock.push(lock_obj);}
- addMsgLog(lock_obj.lockmsg());
- }
- else if (rqst_action =='unlock'){
- var trouve = false;
- var cur_id_obj=-1;
- var i=0;
- if (rqst_obj_type=='waypoint')
- {
- while((!trouve)&&(i
-
-
-
-
-
-
-
-
--------- Paparazzi On the Web ! --------
-
-Welcome,
-<%
- String log = (String) session.getAttribute("login");
- if(log != null) {
- out.print(log + "!");
- String rights = (String) session.getAttribute("rights");
- if (rights.equals("admin")){
- out.println(" |
admin page ");
- }
- out.println(" |
\"pull\" mode ");
- out.println(" |
log out ");
- }
- else
- {
- out.println("");
- }
-
-%>
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-loading...
-
-
-
-
-loading...
-
-
-
-
-
-
Flight Parameters
-
-
-
-
-
- altitude (m)
- 0
-
-
- height
- 0 m
-
-
- battery level
- 0V
-
-
- speed
- 0 km/h
-
-
- GPS status
- OFF
-
-
- vertical speed
- 0 m/s
-
-
- engine power
- 0%
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-<%
- out.println("");
-%>
-
-
-<%-- --%>
-
-
\ No newline at end of file
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/META-INF/MANIFEST.MF b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/META-INF/MANIFEST.MF
deleted file mode 100755
index 254272e1c0..0000000000
--- a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/META-INF/MANIFEST.MF
+++ /dev/null
@@ -1,3 +0,0 @@
-Manifest-Version: 1.0
-Class-Path:
-
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/classes/pushlet.properties b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/classes/pushlet.properties
deleted file mode 100755
index ba656e138f..0000000000
--- a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/classes/pushlet.properties
+++ /dev/null
@@ -1,128 +0,0 @@
-#
-# Pushlet configuration.
-# Place this file in the CLASSPATH (e.g. WEB-INF/classes) or directly under WEB-INF.
-#
-# $Id: pushlet.properties,v 1.13 2007/12/07 12:57:40 justb Exp $
-#
-
-#
-#
-#
-config.version=1.0.2
-
-#
-# CLASS FACTORY SPECIFICATION
-#
-# Change these if you want to override any of the core classes
-# within the Pushlet framework with your own custom classes.
-#
-# Examples:
-# - custom SessionManager for authorisation
-# - maintain lists of active subjects (topics)
-# - send events on subscription
-# - plug in custom logging like log4j
-# Note that you must maintain the semantics of each class !
-# Below are the default properties for the core classes.
-controller.class=nl.justobjects.pushlet.core.Controller
-dispatcher.class=nl.justobjects.pushlet.core.Dispatcher
-logger.class=nl.justobjects.pushlet.util.Log4jLogger
-# logger.class=nl.justobjects.pushlet.util.DefaultLogger
-sessionmanager.class=nl.justobjects.pushlet.core.SessionManager
-session.class=nl.justobjects.pushlet.core.Session
-subscriber.class=nl.justobjects.pushlet.core.Subscriber
-subscription.class=nl.justobjects.pushlet.core.Subscription
-
-# sessionmanager.maxsessions=200
-
-#
-# DISPATCHER
-#
-
-
-# TODO: allow properties to be maintained in
-# a user dir
-# config.redirect=/etc/pushlet.properties
-
-#
-# LOGGING
-#
-
-# log level (trace(6) debug(5) info (4), warn(3), error(2), fatal(1))
-# default is info(4)
-log.level=4
-
-#
-# LOCAL EVENT SOURCES
-#
-
-# should local sources be loaded ?
-sources.activate=true
-
-#
-# SESSION
-#
-
-
-# algoritm to generate session key:
-# values: "randomstring" (default) or "uuid".
-# session.id.generation=uuid
-session.id.generation=randomstring
-
-# length of generated session key when using "randomstring" generation
-session.id.size=10
-
-# Overall session lease time in minutes
-# Mainly used for clients that do not perform
-# listening, e.g. when publishing only.
-session.timeout.mins=5
-
-#
-# EVENT QUEUE
-#
-# Properties for per-client data event queue
-
-# Size for
-queue.size=24
-queue.read.timeout.millis=20000
-queue.write.timeout.millis=20
-
-#
-# LISTENING MODE
-#
-
-# You may force all clients to use pull mode
-# for scalability
-listen.force.pull.all=false
-
-#
-# Comma-separated list of User Agent substrings.
-# Force these browsers to use pull mode, since they
-# don't support JS streaming, matching is done using
-# String.indexOf() with lowercased agent strings
-# use multiple criteria with &.
-#
-listen.force.pull.agents=safari
-
-#
-# PULL MODE
-#
-
-# time server should wait on refresing pull client
-pull.refresh.timeout.millis=45000
-
-# minimum/maximum wait time client should wait before refreshing
-# server provides a random time between these values
-pull.refresh.wait.min.millis=2000
-pull.refresh.wait.max.millis=6000
-
-#
-# POLL MODE
-#
-
-# time server should wait on refresing poll client
-poll.refresh.timeout.millis=60000
-
-# minimum/maximum wait time client should wait before refreshing
-# server provides a random time between these values
-poll.refresh.wait.min.millis=6000
-poll.refresh.wait.max.millis=10000
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/classes/sources.properties b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/classes/sources.properties
deleted file mode 100755
index e8aeb36a43..0000000000
--- a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/classes/sources.properties
+++ /dev/null
@@ -1,28 +0,0 @@
-#
-# Properties file for EventSource objects to be instantiated.
-#
-# Place this file in the CLASSPATH (e.g. WEB-INF/classes) or directly under WEB-INF.
-#
-# $Id: sources.properties,v 1.2 2007/11/10 14:12:16 justb Exp $
-#
-# Each EventSource is defined as =
-# 1. should be unique within this file but may be any name
-# 2. is the full class name
-#
-#
-# Define Pull Sources here. These classes must be derived from
-# nl.justobjects.pushlet.core.EventPullSource
-# Inner classes are separated with a $ sign from the outer class.
-#source1=nl.justobjects.pushlet.test.TestEventPullSources$TemperatureEventPullSource
-#source2=nl.justobjects.pushlet.test.TestEventPullSources$SystemStatusEventPullSource
-#source3=nl.justobjects.pushlet.test.TestEventPullSources$PushletStatusEventPullSource
-#source4=nl.justobjects.pushlet.test.TestEventPullSources$AEXStocksEventPullSource
-#source5=nl.justobjects.pushlet.test.TestEventPullSources$WebPresentationEventPullSource
-#source6=nl.justobjects.pushlet.test.TestEventPullSources$PingEventPullSource
-
-source1=pow.webserver.Serveur$IvyEventSource
-
-# TO BE DONE IN NEXT VERSION
-# define Push Sources here. These must implement the interface
-# nl.justobjects.pushlet.core.EventSource
-
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/lib/commons-codec-1.4.jar b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/lib/commons-codec-1.4.jar
deleted file mode 100755
index 458d432da8..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/lib/commons-codec-1.4.jar and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/lib/commons-fileupload-1.2.1.jar b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/lib/commons-fileupload-1.2.1.jar
deleted file mode 100755
index aa209b3887..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/lib/commons-fileupload-1.2.1.jar and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/lib/commons-httpclient-3.1.jar b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/lib/commons-httpclient-3.1.jar
deleted file mode 100755
index 7c59774aed..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/lib/commons-httpclient-3.1.jar and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/lib/commons-io-1.4.jar b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/lib/commons-io-1.4.jar
deleted file mode 100755
index 133dc6cb35..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/lib/commons-io-1.4.jar and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/lib/commons-logging-1.1.1.jar b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/lib/commons-logging-1.1.1.jar
deleted file mode 100755
index 8758a96b70..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/lib/commons-logging-1.1.1.jar and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/lib/jdom.jar b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/lib/jdom.jar
deleted file mode 100755
index 65a1b3f737..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/lib/jdom.jar and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/lib/log4j-1.2.16.jar b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/lib/log4j-1.2.16.jar
deleted file mode 100755
index 3f9d847618..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/lib/log4j-1.2.16.jar and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/lib/mysql-connector-java-5.1.12-bin.jar b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/lib/mysql-connector-java-5.1.12-bin.jar
deleted file mode 100755
index af5847eed4..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/lib/mysql-connector-java-5.1.12-bin.jar and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/lib/pushlet.jar b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/lib/pushlet.jar
deleted file mode 100755
index 6dc1c22e21..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/lib/pushlet.jar and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/web.xml b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/web.xml
deleted file mode 100755
index 16b18bbc06..0000000000
--- a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/WEB-INF/web.xml
+++ /dev/null
@@ -1,54 +0,0 @@
-
-
- ServletPow
-
- index.html
- index.htm
- index.jsp
- default.html
- default.htm
- default.jsp
-
-
-
- Greeting
- Greeting
- pow.webserver.Greeting
-
-
- Greeting
- /Greeting.srv
-
-
-
- Pushlet
- Pushlet
- nl.justobjects.pushlet.servlet.Pushlet
- 1
-
-
- Pushlet
- /pushlet.srv
-
-
-
- Ivy2TomcatHttpServer
- Ivy2TomcatHttpServer
- pow.webserver.Ivy2TomcatHttpServer
- 1
-
-
- Ivy2TomcatHttpServer
- /Ivy2TomcatHttpServer.srv
-
-
-
- ajaxRqst
- ajaxRqst
- pow.webserver.AjaxRqst
-
-
- ajaxRqst
- /ajaxRqst.srv
-
-
\ No newline at end of file
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/admin.jsp b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/admin.jsp
deleted file mode 100755
index 77c8c2fa9c..0000000000
--- a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/admin.jsp
+++ /dev/null
@@ -1,790 +0,0 @@
-<%@ page language="java" contentType="text/html; charset=ISO-8859-1"
- pageEncoding="ISO-8859-1"%>
-<%@page import="java.util.*,pow.webserver.UserTab,pow.webserver.User,javax.xml.parsers.*, org.w3c.dom.*,org.xml.sax.*,java.io.*" %>
-<%
-// recuperation des données sur le serveur
- String default_folder = this.getServletContext().getRealPath("");
- UserTab logTab = UserTab.unserialize(default_folder + "/conf/"+"userTable.tbl");
- Iterator itr=logTab.getLoginIterator();
- //
- UserTab logIvyTab = UserTab.unserialize(default_folder + "/conf/"+"userIvyTable.tbl");
- Iterator itrIvy=logIvyTab.getLoginIterator();
- // lecture liste des noms de drones ds immat.xml
- DocumentBuilderFactory fabrique = DocumentBuilderFactory.newInstance();
- // création d'un constructeur de documents
- DocumentBuilder constructeur = fabrique.newDocumentBuilder();
- // lecture du contenu d'un fichier XML avec DOM
- File xml = new File(default_folder + "/conf/"+"immat.xml");
- Document document = constructeur.parse(xml);
-
-
-%>
-
-
-
-
-
-
-
-Administration page
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/conf/immat.xml b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/conf/immat.xml
deleted file mode 100755
index 42965d30c1..0000000000
--- a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/conf/immat.xml
+++ /dev/null
@@ -1,7 +0,0 @@
-
-
-
-
-
-
-
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/conf/pow_conf.xml b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/conf/pow_conf.xml
deleted file mode 100755
index 947a6b35b7..0000000000
--- a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/conf/pow_conf.xml
+++ /dev/null
@@ -1,24 +0,0 @@
-
-
- 8535
- 8536
- 1024
- 10000
- pow_sql
- true silent pow_user
- pwdpow_user
- admin
- admin@pow.fr
-
- 1200000
-
- 60000
-
-
-10000
-
-5000
-
-30000
- 30000
-
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/help.jsp b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/help.jsp
deleted file mode 100755
index cc8eedffa0..0000000000
--- a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/help.jsp
+++ /dev/null
@@ -1,58 +0,0 @@
-<%@page import="pow.webserver.Conf" %>
-
-
-
-
-
- Welcome to Paparazzi On the Web
-
-
-
-
-
-
-
-
-
-Welcome on Paparazzi On the Web help page.
- This page is here to help you to use the Paparazzi On the Web (POW) application. POW is a part of Paparazzi which is a open-project of civil UAV control. With Paparazzi On the Web, you are able to observe and eventually control the UAVs of the system Paparazzi thanks to a browser with your internet connexion. To learn more about Paparazzi project, click here .
-Paparazzi On the Web is optimised for Firefox browser. If you want to have a better performance, you can dowload Firefox here .
-Start page
-If you have a login and a password on Paparazzi On the Web, you can use them to enter the application. Else, click on enter as a guest.
-
-Main page
-You are now on Paparazzi On the Web main page. You can see several boxes. We will describe all those boxes and show you the interactions you can have with them.
-
-Choose an aircraft
-Here you can select the aircraft you want to observe or control. Click on the arrow to see the list of the aircrafts that are available. Click on the aircraft you want to interacte with.
-You can also choose to activate or desactivate tracking. When tracking is activated, the map is always centered on the selected aircraft.
-
-
-
-The map
-On the map, you can see all the aircrafts that are available. If tracking is activated, the map will be centered on the selected aircraft. There are also red lozenges which are the waypoints of the selected UAV. If you are autentified on Paparazzi On the Web and if your profile gives you the authorization, you can move those waypoints with drag and drop.
-
-The flight plan
-Through this box, you can obtain some informations on the flight plan that is followed by the selected plane. If you move your cursor on the informations tab, you obtain informations on the flightplan. If you put your cursor on the "active block" tab, you can see the list of the flight plan blocks that the plane can follow. The active block is in red. If you are authentified and if you have the authorization, you can change the active block by clicking on its name. The waypoints tab gives you informations on the different waypoints. If you are authentified and if you have the authorization, you can move those waypoints by clicking on "move waypoint". In this case, a new windows is openned where you indicate where you want to move the waypoint.
-You can also move a waypoint by dragging the icon on the map to its new position.
-
-The flight parameters
-This box displays the flight parameters of the selected aircraft. When a value is out of its normal range, it becomes red.
-
-The settings
-In this box you can, if you are allowed to control the selected aircraft, you change some settings of this aircraft by clicking on the name of the setting.
-
-Any problem ?
-If you have any display problem, such as a ghost aircraft icon, or an aircraft that is not displayed in the aircrafts list, you might resolve it by reloading the page by pressing F5 key.
-For any other problem, please inform
-<%
-out.print("the administrator "); %>.
-Click here to return to the homepage.
-
-
-
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/idError.html b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/idError.html
deleted file mode 100755
index a15edb3f6d..0000000000
--- a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/idError.html
+++ /dev/null
@@ -1,14 +0,0 @@
-
-
-
-
- Welcome to Paparazzi On the Web
-
-
-
-
-
-Sorry, an error occured during your identification. Click here to return on the homepage.
-
-
\ No newline at end of file
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/base.gif b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/base.gif
deleted file mode 100755
index 9ac0b117b0..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/base.gif and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/cd.gif b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/cd.gif
deleted file mode 100755
index 7503819404..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/cd.gif and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/empty.gif b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/empty.gif
deleted file mode 100755
index b5cf52378f..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/empty.gif and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/folder.gif b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/folder.gif
deleted file mode 100755
index eb129763dc..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/folder.gif and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/folderopen.gif b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/folderopen.gif
deleted file mode 100755
index c5c31102d5..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/folderopen.gif and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/globe.gif b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/globe.gif
deleted file mode 100755
index 57123d0e69..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/globe.gif and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/imgfolder.gif b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/imgfolder.gif
deleted file mode 100755
index e6d880347f..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/imgfolder.gif and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/join.gif b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/join.gif
deleted file mode 100755
index 34dd47610a..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/join.gif and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/joinbottom.gif b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/joinbottom.gif
deleted file mode 100755
index 48b81c80a9..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/joinbottom.gif and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/line.gif b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/line.gif
deleted file mode 100755
index 1a259eea00..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/line.gif and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/minus.gif b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/minus.gif
deleted file mode 100755
index 3d212a97ae..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/minus.gif and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/minusbottom.gif b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/minusbottom.gif
deleted file mode 100755
index dc3198be27..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/minusbottom.gif and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/musicfolder.gif b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/musicfolder.gif
deleted file mode 100755
index f620789feb..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/musicfolder.gif and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/nolines_minus.gif b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/nolines_minus.gif
deleted file mode 100755
index 2592ac20f3..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/nolines_minus.gif and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/nolines_plus.gif b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/nolines_plus.gif
deleted file mode 100755
index f258ce211a..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/nolines_plus.gif and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/page.gif b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/page.gif
deleted file mode 100755
index 42d7318c5d..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/page.gif and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/plus.gif b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/plus.gif
deleted file mode 100755
index b2c997233b..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/plus.gif and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/plusbottom.gif b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/plusbottom.gif
deleted file mode 100755
index b5671d891a..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/plusbottom.gif and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/question.gif b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/question.gif
deleted file mode 100755
index dd4e685078..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/question.gif and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/trash.gif b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/trash.gif
deleted file mode 100755
index cfa0f000e1..0000000000
Binary files a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/img/trash.gif and /dev/null differ
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/index.jsp b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/index.jsp
deleted file mode 100755
index aad5c605b5..0000000000
--- a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/index.jsp
+++ /dev/null
@@ -1,69 +0,0 @@
-
-
-
-
- Welcome to Paparazzi On the Web
-
-
-
-
-
-
-
-
-
-
-
-Welcome to Paparazzi On The Web !
-
-
-
-
-
-
-
-or
-
-
-
-
-help ?
-
-
\ No newline at end of file
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/js/DOMImplementation.js b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/js/DOMImplementation.js
deleted file mode 100755
index 75c17e11d6..0000000000
--- a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/js/DOMImplementation.js
+++ /dev/null
@@ -1,24 +0,0 @@
-function DOMImplementation(sUrl, fCallback) {
- var dom;
- if(window.ActiveXObject) {
- dom = new ActiveXObject("Microsoft.XMLDOM");
- dom.onreadystatechange = function() {
- if(dom.readyState == 4) {
- //alert("win "+sUrl+" loaded");
- fCallback(dom);
- }
- };
- }
- else if(document.implementation && document.implementation.createDocument) {
- dom = document.implementation.createDocument("", "", null);
- dom.onload = function() {
- //alert("other "+sUrl+" loaded");
- fCallback(dom);
- }
- }
- else {
- alert("Votre navigateur ne gère pas l'importation de fichiers XML");
- return;
- }
- dom.load(sUrl);
-}
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/js/XHR_object.js b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/js/XHR_object.js
deleted file mode 100755
index 8f3f022733..0000000000
--- a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/js/XHR_object.js
+++ /dev/null
@@ -1,20 +0,0 @@
-function getXMLHttpRequest() {
- var xhr = null;
-
- if (window.XMLHttpRequest || window.ActiveXObject) {
- if (window.ActiveXObject) {
- try {
- xhr = new ActiveXObject("Msxml2.XMLHTTP");
- } catch(e) {
- xhr = new ActiveXObject("Microsoft.XMLHTTP");
- }
- } else {
- xhr = new XMLHttpRequest();
- }
- } else {
- alert("Votre navigateur ne supporte pas l'objet XMLHTTPRequest...");
- return null;
- }
-
- return xhr;
-}
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/js/browserdetect.js b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/js/browserdetect.js
deleted file mode 100755
index a8674db57f..0000000000
--- a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/js/browserdetect.js
+++ /dev/null
@@ -1,97 +0,0 @@
-// Browser Detect Lite v2.1.4
-// http://www.dithered.com/javascript/browser_detect/index.html
-// modified by Chris Nott (chris@NOSPAMdithered.com - remove NOSPAM)
-
-
-function BrowserDetectLite() {
- var ua = navigator.userAgent.toLowerCase();
-
- // browser name
- this.isGecko = (ua.indexOf('gecko') != -1 && ua.indexOf('safari') == -1);
- this.isMozilla = (this.isGecko && ua.indexOf('gecko/') + 14 == ua.length);
- this.isNS = ( (this.isGecko) ? (ua.indexOf('netscape') != -1) : ( (ua.indexOf('mozilla') != -1) && (ua.indexOf('spoofer') == -1) && (ua.indexOf('compatible') == -1) && (ua.indexOf('opera') == -1) && (ua.indexOf('webtv') == -1) && (ua.indexOf('hotjava') == -1) ) );
- this.isIE = ( (ua.indexOf('msie') != -1) && (ua.indexOf('opera') == -1) && (ua.indexOf('webtv') == -1) );
- this.isSafari = (ua.indexOf('safari') != - 1);
- this.isOpera = (ua.indexOf('opera') != -1);
- this.isKonqueror = (ua.indexOf('konqueror') != -1 && !this.isSafari);
- this.isIcab = (ua.indexOf('icab') != -1);
- this.isAol = (ua.indexOf('aol') != -1);
-
- // spoofing and compatible browsers
- this.isIECompatible = ( (ua.indexOf('msie') != -1) && !this.isIE);
- this.isNSCompatible = ( (ua.indexOf('mozilla') != -1) && !this.isNS && !this.isMozilla);
-
- // browser version
- this.versionMinor = parseFloat(navigator.appVersion);
-
- // correct version number
- if (this.isNS && this.isGecko) {
- this.versionMinor = parseFloat( ua.substring( ua.lastIndexOf('/') + 1 ) );
- }
- else if (this.isIE && this.versionMinor >= 4) {
- this.versionMinor = parseFloat( ua.substring( ua.indexOf('msie ') + 5 ) );
- }
- else if (this.isMozilla) {
- this.versionMinor = parseFloat( ua.substring( ua.indexOf('rv:') + 3 ) );
- }
- else if (this.isSafari) {
- this.versionMinor = parseFloat( ua.substring( ua.lastIndexOf('/') + 1 ) );
- }
- else if (this.isOpera) {
- if (ua.indexOf('opera/') != -1) {
- this.versionMinor = parseFloat( ua.substring( ua.indexOf('opera/') + 6 ) );
- }
- else {
- this.versionMinor = parseFloat( ua.substring( ua.indexOf('opera ') + 6 ) );
- }
- }
- else if (this.isKonqueror) {
- this.versionMinor = parseFloat( ua.substring( ua.indexOf('konqueror/') + 10 ) );
- }
- else if (this.isIcab) {
- if (ua.indexOf('icab/') != -1) {
- this.versionMinor = parseFloat( ua.substring( ua.indexOf('icab/') + 6 ) );
- }
- else {
- this.versionMinor = parseFloat( ua.substring( ua.indexOf('icab ') + 6 ) );
- }
- }
-
- this.versionMajor = parseInt(this.versionMinor);
- this.geckoVersion = ( (this.isGecko) ? ua.substring( (ua.lastIndexOf('gecko/') + 6), (ua.lastIndexOf('gecko/') + 14) ) : -1 );
-
- // dom support
- this.isDOM1 = (document.getElementById);
- this.isDOM2Event = (document.addEventListener && document.removeEventListener);
-
- // css compatibility mode
- this.mode = document.compatMode ? document.compatMode : 'BackCompat';
-
- // platform
- this.isWin = (ua.indexOf('win') != -1);
- this.isWin32 = (this.isWin && ( ua.indexOf('95') != -1 || ua.indexOf('98') != -1 || ua.indexOf('nt') != -1 || ua.indexOf('win32') != -1 || ua.indexOf('32bit') != -1 || ua.indexOf('xp') != -1) );
- this.isMac = (ua.indexOf('mac') != -1);
- this.isUnix = (ua.indexOf('unix') != -1 || ua.indexOf('sunos') != -1 || ua.indexOf('bsd') != -1 || ua.indexOf('x11') != -1)
- this.isLinux = (ua.indexOf('linux') != -1);
-
- // specific browser shortcuts
- this.isNS4x = (this.isNS && this.versionMajor == 4);
- this.isNS40x = (this.isNS4x && this.versionMinor < 4.5);
- this.isNS47x = (this.isNS4x && this.versionMinor >= 4.7);
- this.isNS4up = (this.isNS && this.versionMinor >= 4);
- this.isNS6x = (this.isNS && this.versionMajor == 6);
- this.isNS6up = (this.isNS && this.versionMajor >= 6);
- this.isNS7x = (this.isNS && this.versionMajor == 7);
- this.isNS7up = (this.isNS && this.versionMajor >= 7);
-
- this.isIE4x = (this.isIE && this.versionMajor == 4);
- this.isIE4up = (this.isIE && this.versionMajor >= 4);
- this.isIE5x = (this.isIE && this.versionMajor == 5);
- this.isIE55 = (this.isIE && this.versionMinor == 5.5);
- this.isIE5up = (this.isIE && this.versionMajor >= 5);
- this.isIE6x = (this.isIE && this.versionMajor == 6);
- this.isIE6up = (this.isIE && this.versionMajor >= 6);
-
- this.isIE4xMac = (this.isIE4x && this.isMac);
-}
-var browser = new BrowserDetectLite();
diff --git a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/js/curvycorners.js b/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/js/curvycorners.js
deleted file mode 100755
index 511c78dd9d..0000000000
--- a/sw/in_progress/pow/ServletPow/eclipse ServletPow/WebContent/js/curvycorners.js
+++ /dev/null
@@ -1 +0,0 @@
-function browserdetect(){var A=navigator.userAgent.toLowerCase();this.isIE=A.indexOf("msie")>-1;this.ieVer=this.isIE?/msie\s(\d\.\d)/.exec(A)[1]:0;this.isMoz=A.indexOf("firefox")!=-1;this.isSafari=A.indexOf("safari")!=-1;this.quirksMode=this.isIE&&(!document.compatMode||document.compatMode.indexOf("BackCompat")>-1);this.isOp="opera" in window;this.isWebKit=A.indexOf("webkit")!=-1;if(this.isIE){this.get_style=function(D,F){if(!(F in D.currentStyle)){return""}var C=/^([\d.]+)(\w*)/.exec(D.currentStyle[F]);if(!C){return D.currentStyle[F]}if(C[1]==0){return"0"}if(C[2]&&C[2]!=="px"){var B=D.style.left;var E=D.runtimeStyle.left;D.runtimeStyle.left=D.currentStyle.left;D.style.left=C[1]+C[2];C[0]=D.style.pixelLeft;D.style.left=B;D.runtimeStyle.left=E}return C[0]}}else{this.get_style=function(B,C){C=C.replace(/([a-z])([A-Z])/g,"$1-$2").toLowerCase();return document.defaultView.getComputedStyle(B,"").getPropertyValue(C)}}}var curvyBrowser=new browserdetect;if(curvyBrowser.isIE){try{document.execCommand("BackgroundImageCache",false,true)}catch(e){}}function curvyCnrSpec(A){this.selectorText=A;this.tlR=this.trR=this.blR=this.brR=0;this.tlu=this.tru=this.blu=this.bru="";this.antiAlias=true}curvyCnrSpec.prototype.setcorner=function(B,C,A,D){if(!B){this.tlR=this.trR=this.blR=this.brR=parseInt(A);this.tlu=this.tru=this.blu=this.bru=D}else{propname=B.charAt(0)+C.charAt(0);this[propname+"R"]=parseInt(A);this[propname+"u"]=D}};curvyCnrSpec.prototype.get=function(D){if(/^(t|b)(l|r)(R|u)$/.test(D)){return this[D]}if(/^(t|b)(l|r)Ru$/.test(D)){var C=D.charAt(0)+D.charAt(1);return this[C+"R"]+this[C+"u"]}if(/^(t|b)Ru?$/.test(D)){var B=D.charAt(0);B+=this[B+"lR"]>this[B+"rR"]?"l":"r";var A=this[B+"R"];if(D.length===3&&D.charAt(2)==="u"){A+=this[B="u"]}return A}throw new Error("Don't recognize property "+D)};curvyCnrSpec.prototype.radiusdiff=function(A){if(A!=="t"&&A!=="b"){throw new Error("Param must be 't' or 'b'")}return Math.abs(this[A+"lR"]-this[A+"rR"])};curvyCnrSpec.prototype.setfrom=function(A){this.tlu=this.tru=this.blu=this.bru="px";if("tl" in A){this.tlR=A.tl.radius}if("tr" in A){this.trR=A.tr.radius}if("bl" in A){this.blR=A.bl.radius}if("br" in A){this.brR=A.br.radius}if("antiAlias" in A){this.antiAlias=A.antiAlias}};curvyCnrSpec.prototype.cloneOn=function(G){var E=["tl","tr","bl","br"];var H=0;var C,A;for(C in E){if(!isNaN(C)){A=this[E[C]+"u"];if(A!==""&&A!=="px"){H=new curvyCnrSpec;break}}}if(!H){H=this}else{var B,D,F=curvyBrowser.get_style(G,"left");for(C in E){if(!isNaN(C)){B=E[C];A=this[B+"u"];D=this[B+"R"];if(A!=="px"){var F=G.style.left;G.style.left=D+A;D=G.style.pixelLeft;G.style.left=F}H[B+"R"]=D;H[B+"u"]="px"}}G.style.left=F}return H};curvyCnrSpec.prototype.radiusSum=function(A){if(A!=="t"&&A!=="b"){throw new Error("Param must be 't' or 'b'")}return this[A+"lR"]+this[A+"rR"]};curvyCnrSpec.prototype.radiusCount=function(A){var B=0;if(this[A+"lR"]){++B}if(this[A+"rR"]){++B}return B};curvyCnrSpec.prototype.cornerNames=function(){var A=[];if(this.tlR){A.push("tl")}if(this.trR){A.push("tr")}if(this.blR){A.push("bl")}if(this.brR){A.push("br")}return A};function operasheet(C){var A=document.styleSheets.item(C).ownerNode.text;A=A.replace(/\/\*(\n|\r|.)*?\*\//g,"");var D=new RegExp("^s*([\\w.#][-\\w.#, ]+)[\\n\\s]*\\{([^}]+border-((top|bottom)-(left|right)-)?radius[^}]*)\\}","mg");var G;this.rules=[];while((G=D.exec(A))!==null){var F=new RegExp("(..)border-((top|bottom)-(left|right)-)?radius:\\s*([\\d.]+)(in|em|px|ex|pt)","g");var E,B=new curvyCnrSpec(G[1]);while((E=F.exec(G[2]))!==null){if(E[1]!=="z-"){B.setcorner(E[3],E[4],E[5],E[6])}}this.rules.push(B)}}operasheet.contains_border_radius=function(A){return/border-((top|bottom)-(left|right)-)?radius/.test(document.styleSheets.item(A).ownerNode.text)};function curvyCorners(){var G,D,E,B,J;if(typeof arguments[0]!=="object"){throw curvyCorners.newError("First parameter of curvyCorners() must be an object.")}if(arguments[0] instanceof curvyCnrSpec){B=arguments[0];if(!B.selectorText&&typeof arguments[1]==="string"){B.selectorText=arguments[1]}}else{if(typeof arguments[1]!=="object"&&typeof arguments[1]!=="string"){throw curvyCorners.newError("Second parameter of curvyCorners() must be an object or a class name.")}D=arguments[1];if(typeof D!=="string"){D=""}if(D!==""&&D.charAt(0)!=="."&&"autoPad" in arguments[0]){D="."+D}B=new curvyCnrSpec(D);B.setfrom(arguments[0])}if(B.selectorText){J=0;var I=B.selectorText.replace(/\s+$/,"").split(/,\s*/);E=new Array;function A(M){var L=M.split("#");return(L.length===2?"#":"")+L.pop()}for(G=0;G7?curvyBrowser.get_style(this.box,"filter"):null;var H=this.spec.get("tR");var M=this.spec.get("bR");var W=function(f){if(typeof f==="number"){return f}if(typeof f!=="string"){throw new Error("unexpected styleToNPx type "+typeof f)}var d=/^[-\d.]([a-z]+)$/.exec(f);if(d&&d[1]!="px"){throw new Error("Unexpected unit "+d[1])}if(isNaN(f=parseInt(f))){f=0}return f};var T=function(d){return d<=0?"0":d+"px"};try{this.borderWidth=W(b);this.borderWidthB=W(J);this.borderWidthL=W(D);this.borderWidthR=W(B);this.boxColour=curvyObject.format_colour(E);this.topPadding=W(Z);this.bottomPadding=W(c);this.leftPadding=W(Q);this.rightPadding=W(a);this.boxWidth=K;this.boxHeight=this.box.clientHeight;this.borderColour=curvyObject.format_colour(I);this.borderColourB=curvyObject.format_colour(G);this.borderColourL=curvyObject.format_colour(A);this.borderString=this.borderWidth+"px solid "+this.borderColour;this.borderStringB=this.borderWidthB+"px solid "+this.borderColourB;this.backgroundImage=((C!="none")?C:"");this.backgroundRepeat=Y}catch(X){throw this.newError("getMessage" in X?X.getMessage():X.message)}var F=this.boxHeight;var V=K;if(curvyBrowser.isOp){R=W(R);P=W(P);if(R){var N=V+this.borderWidthL+this.borderWidthR;if(R>N){R=N}R=(N/R*100)+"%"}if(P){var N=F+this.borderWidth+this.borderWidthB;if(P>N){P=N}P=(N/P*100)+"%"}}if(curvyBrowser.quirksMode){}else{this.boxWidth-=this.leftPadding+this.rightPadding;this.boxHeight-=this.topPadding+this.bottomPadding}this.contentContainer=document.createElement("div");if(filter){this.contentContainer.style.filter=filter}while(this.box.firstChild){this.contentContainer.appendChild(this.box.removeChild(this.box.firstChild))}if(O!="absolute"){this.box.style.position="relative"}this.box.style.padding="0";this.box.style.border=this.box.style.backgroundImage="none";this.box.style.backgroundColor="transparent";this.box.style.width=(V+this.borderWidthL+this.borderWidthR)+"px";this.box.style.height=(F+this.borderWidth+this.borderWidthB)+"px";var L=document.createElement("div");L.style.position="absolute";if(filter){L.style.filter=filter}if(curvyBrowser.quirksMode){L.style.width=(V+this.borderWidthL+this.borderWidthR)+"px"}else{L.style.width=V+"px"}L.style.height=T(F+this.borderWidth+this.borderWidthB-H-M);L.style.padding="0";L.style.top=H+"px";L.style.left="0";if(this.borderWidthL){L.style.borderLeft=this.borderWidthL+"px solid "+this.borderColourL}if(this.borderWidth&&!H){L.style.borderTop=this.borderWidth+"px solid "+this.borderColour}if(this.borderWidthR){L.style.borderRight=this.borderWidthR+"px solid "+this.borderColourL}if(this.borderWidthB&&!M){L.style.borderBottom=this.borderWidthB+"px solid "+this.borderColourB}L.style.backgroundColor=E;L.style.backgroundImage=this.backgroundImage;L.style.backgroundRepeat=this.backgroundRepeat;this.shell=this.box.appendChild(L);K=curvyBrowser.get_style(this.shell,"width");if(K===""||K==="auto"||K.indexOf("%")!==-1){throw this.newError("Shell width is "+K)}this.boxWidth=(K!=""&&K!="auto"&&K.indexOf("%")==-1)?parseInt(K):this.shell.clientWidth;this.applyCorners=function(){if(this.backgroundObject){var w=function(AO,i,t){if(AO===0){return 0}var k;if(AO==="right"||AO==="bottom"){return t-i}if(AO==="center"){return(t-i)/2}if(AO.indexOf("%")>0){return(t-i)*100/parseInt(AO)}return W(AO)};this.backgroundPosX=w(R,this.backgroundObject.width,V);this.backgroundPosY=w(P,this.backgroundObject.height,F)}else{if(this.backgroundImage){this.backgroundPosX=W(R);this.backgroundPosY=W(P)}}if(H){v=document.createElement("div");v.style.width=this.boxWidth+"px";v.style.fontSize="1px";v.style.overflow="hidden";v.style.position="absolute";v.style.paddingLeft=this.borderWidth+"px";v.style.paddingRight=this.borderWidth+"px";v.style.height=H+"px";v.style.top=-H+"px";v.style.left=-this.borderWidthL+"px";this.topContainer=this.shell.appendChild(v)}if(M){var v=document.createElement("div");v.style.width=this.boxWidth+"px";v.style.fontSize="1px";v.style.overflow="hidden";v.style.position="absolute";v.style.paddingLeft=this.borderWidthB+"px";v.style.paddingRight=this.borderWidthB+"px";v.style.height=M+"px";v.style.bottom=-M+"px";v.style.left=-this.borderWidthL+"px";this.bottomContainer=this.shell.appendChild(v)}var AG=this.spec.cornerNames();for(var AK in AG){if(!isNaN(AK)){var AC=AG[AK];var AD=this.spec[AC+"R"];var AE,AH,j,AF;if(AC=="tr"||AC=="tl"){AE=this.borderWidth;AH=this.borderColour;AF=this.borderWidth}else{AE=this.borderWidthB;AH=this.borderColourB;AF=this.borderWidthB}j=AD-AF;var u=document.createElement("div");u.style.height=this.spec.get(AC+"Ru");u.style.width=this.spec.get(AC+"Ru");u.style.position="absolute";u.style.fontSize="1px";u.style.overflow="hidden";var r,q,p;var n=filter?parseInt(/alpha\(opacity.(\d+)\)/.exec(filter)[1]):100;for(r=0;r=j)?-1:Math.floor(Math.sqrt(Math.pow(j,2)-Math.pow(r+1,2)))-1;if(j!=AD){var h=(r>=j)?-1:Math.ceil(Math.sqrt(Math.pow(j,2)-Math.pow(r,2)));var f=(r+1>=AD)?-1:Math.floor(Math.sqrt(Math.pow(AD,2)-Math.pow((r+1),2)))-1}var d=(r>=AD)?-1:Math.ceil(Math.sqrt(Math.pow(AD,2)-Math.pow(r,2)));if(m>-1){this.drawPixel(r,0,this.boxColour,n,(m+1),u,true,AD)}if(j!=AD){if(this.spec.antiAlias){for(q=m+1;q=30,AD)}else{if(this.boxColour!=="transparent"){var AB=curvyObject.BlendColour(this.boxColour,AH,curvyObject.pixelFraction(r,q,j));this.drawPixel(r,q,AB,n,1,u,false,AD)}else{this.drawPixel(r,q,AH,n>>1,1,u,false,AD)}}}if(f>=h){if(h==-1){h=0}this.drawPixel(r,h,AH,n,(f-h+1),u,false,0)}p=AH;q=f}else{if(f>m){this.drawPixel(r,(m+1),AH,n,(f-m),u,false,0)}}}else{p=this.boxColour;q=m}if(this.spec.antiAlias){while(++q>>4]+""+A[B&15]};curvyObject.BlendColour=function(L,J,G){if(L==="transparent"||J==="transparent"){throw this.newError("Cannot blend with transparent")}if(L.charAt(0)!=="#"){L=curvyObject.format_colour(L)}if(J.charAt(0)!=="#"){J=curvyObject.format_colour(J)}var D=parseInt(L.substr(1,2),16);var K=parseInt(L.substr(3,2),16);var F=parseInt(L.substr(5,2),16);var C=parseInt(J.substr(1,2),16);var I=parseInt(J.substr(3,2),16);var E=parseInt(J.substr(5,2),16);if(G>1||G<0){G=1}var H=Math.round((D*G)+(C*(1-G)));if(H>255){H=255}if(H<0){H=0}var B=Math.round((K*G)+(I*(1-G)));if(B>255){B=255}if(B<0){B=0}var A=Math.round((F*G)+(E*(1-G)));if(A>255){A=255}if(A<0){A=0}return"#"+curvyObject.IntToHex(H)+curvyObject.IntToHex(B)+curvyObject.IntToHex(A)};curvyObject.pixelFraction=function(H,G,A){var J;var E=A*A;var B=new Array(2);var F=new Array(2);var I=0;var C="";var D=Math.sqrt(E-Math.pow(H,2));if(D>=G&&D<(G+1)){C="Left";B[I]=0;F[I]=D-G;++I}D=Math.sqrt(E-Math.pow(G+1,2));if(D>=H&&D<(H+1)){C+="Top";B[I]=D-H;F[I]=1;++I}D=Math.sqrt(E-Math.pow(H+1,2));if(D>=G&&D<(G+1)){C+="Right";B[I]=1;F[I]=D-G;++I}D=Math.sqrt(E-Math.pow(G,2));if(D>=H&&D<(H+1)){C+="Bottom";B[I]=D-H;F[I]=0}switch(C){case"LeftRight":J=Math.min(F[0],F[1])+((Math.max(F[0],F[1])-Math.min(F[0],F[1]))/2);break;case"TopRight":J=1-(((1-B[0])*(1-F[1]))/2);break;case"TopBottom":J=Math.min(B[0],B[1])+((Math.max(B[0],B[1])-Math.min(B[0],B[1]))/2);break;case"LeftBottom":J=F[0]*B[1]/2;break;default:J=1}return J};curvyObject.rgb2Array=function(A){var B=A.substring(4,A.indexOf(")"));return B.split(", ")};curvyObject.rgb2Hex=function(B){try{var C=curvyObject.rgb2Array(B);var G=parseInt(C[0]);var E=parseInt(C[1]);var A=parseInt(C[2]);var D="#"+curvyObject.IntToHex(G)+curvyObject.IntToHex(E)+curvyObject.IntToHex(A)}catch(F){var H="getMessage" in F?F.getMessage():F.message;throw new Error("Error ("+H+") converting RGB value to Hex in rgb2Hex")}return D};curvyObject.setOpacity=function(F,C){C=(C==100)?99.999:C;if(curvyBrowser.isSafari&&F.tagName!="IFRAME"){var B=curvyObject.rgb2Array(F.style.backgroundColor);var E=parseInt(B[0]);var D=parseInt(B[1]);var A=parseInt(B[2]);F.style.backgroundColor="rgba("+E+", "+D+", "+A+", "+C/100+")"}else{if(typeof F.style.opacity!=="undefined"){F.style.opacity=C/100}else{if(typeof F.style.MozOpacity!=="undefined"){F.style.MozOpacity=C/100}else{if(typeof F.style.filter!="undefined"){F.style.filter="alpha(opacity="+C+")"}else{if(typeof F.style.KHTMLOpacity!="undefined"){F.style.KHTMLOpacity=C/100}}}}}};function addEvent(D,C,B,A){if(D.addEventListener){D.addEventListener(C,B,A);return true}if(D.attachEvent){return D.attachEvent("on"+C,B)}D["on"+C]=B;return false}curvyObject.getComputedColour=function(E){var F=document.createElement("DIV");F.style.backgroundColor=E;document.body.appendChild(F);if(window.getComputedStyle){var D=document.defaultView.getComputedStyle(F,null).getPropertyValue("background-color");F.parentNode.removeChild(F);if(D.substr(0,3)==="rgb"){D=curvyObject.rgb2Hex(D)}return D}else{var A=document.body.createTextRange();A.moveToElementText(F);A.execCommand("ForeColor",false,E);var B=A.queryCommandValue("ForeColor");var C="rgb("+(B&255)+", "+((B&65280)>>8)+", "+((B&16711680)>>16)+")";F.parentNode.removeChild(F);A=null;return curvyObject.rgb2Hex(C)}};curvyObject.format_colour=function(A){if(A!=""&&A!="transparent"){if(A.substr(0,3)==="rgb"){A=curvyObject.rgb2Hex(A)}else{if(A.charAt(0)!=="#"){A=curvyObject.getComputedColour(A)}else{if(A.length===4){A="#"+A.charAt(1)+A.charAt(1)+A.charAt(2)+A.charAt(2)+A.charAt(3)+A.charAt(3)}}}}return A};curvyCorners.getElementsByClass=function(H,F){var E=new Array;if(F===undefined){F=document}H=H.split(".");var A="*";if(H.length===1){A=H[0];H=false}else{if(H[0]){A=H[0]}H=H[1]}var D,C,B;if(A.charAt(0)==="#"){C=document.getElementById(A.substr(1));if(C){E.push(C)}}else{C=F.getElementsByTagName(A);B=C.length;if(H){var G=new RegExp("(^|\\s)"+H+"(\\s|$)");for(D=0;D6){var H=J["-webkit-border-radius"]||0;var K=J["-webkit-border-top-right-radius"]||0;var F=J["-webkit-border-top-left-radius"]||0;var G=J["-webkit-border-bottom-right-radius"]||0;var M=J["-webkit-border-bottom-left-radius"]||0}else{var H=J["webkit-border-radius"]||0;var K=J["webkit-border-top-right-radius"]||0;var F=J["webkit-border-top-left-radius"]||0;var G=J["webkit-border-bottom-right-radius"]||0;var M=J["webkit-border-bottom-left-radius"]||0}if(H||F||K||G||M){var I=new curvyCnrSpec(L.selectorText);if(H){I.setcorner(null,null,parseInt(H),B(H))}else{if(K){I.setcorner("t","r",parseInt(K),B(K))}if(F){I.setcorner("t","l",parseInt(F),B(F))}if(M){I.setcorner("b","l",parseInt(M),B(M))}if(G){I.setcorner("b","r",parseInt(G),B(G))}}curvyCorners(I)}}for(E=0;E';
- if (!this.selectedFound) this.selectedNode = null;
- this.completed = true;
- return str;
-};
-
-// Creates the tree structure
-dTree.prototype.addNode = function(pNode) {
- var str = '';
- var n=0;
- if (this.config.inOrder) n = pNode._ai;
- for (n; n ';
- }
- if (node.url) {
- str += '';
- }
- else if ((!this.config.folderLinks || !node.url) && node._hc && node.pid != this.root.id)
- str += ' ';
- str += node.name;
- if (node.url || ((!this.config.folderLinks || !node.url) && node._hc)) str += ' ';
- str += '';
- if (node._hc) {
- str += '';
- str += this.addNode(node);
- str += '
';
- }
- this.aIndent.pop();
- return str;
-};
-
-// Adds the empty and line icons
-dTree.prototype.indent = function(node, nodeId) {
- var str = '';
- if (this.root.id != node.pid) {
- for (var n=0; n ';
- (node._ls) ? this.aIndent.push(0) : this.aIndent.push(1);
- if (node._hc) {
- str += ' ';
- } else str += ' ';
- }
- return str;
-};
-
-// Checks if a node has any children and if it is the last sibling
-dTree.prototype.setCS = function(node) {
- var lastId;
- for (var n=0; n)[^>]*$|^#([\w-]+)$/,Ua=/^.[^:#\[\.,]*$/,Va=/\S/,
-Wa=/^(\s|\u00A0)+|(\s|\u00A0)+$/g,Xa=/^<(\w+)\s*\/?>(?:<\/\1>)?$/,P=navigator.userAgent,xa=false,Q=[],L,$=Object.prototype.toString,aa=Object.prototype.hasOwnProperty,ba=Array.prototype.push,R=Array.prototype.slice,ya=Array.prototype.indexOf;c.fn=c.prototype={init:function(a,b){var d,f;if(!a)return this;if(a.nodeType){this.context=this[0]=a;this.length=1;return this}if(a==="body"&&!b){this.context=s;this[0]=s.body;this.selector="body";this.length=1;return this}if(typeof a==="string")if((d=Ta.exec(a))&&
-(d[1]||!b))if(d[1]){f=b?b.ownerDocument||b:s;if(a=Xa.exec(a))if(c.isPlainObject(b)){a=[s.createElement(a[1])];c.fn.attr.call(a,b,true)}else a=[f.createElement(a[1])];else{a=sa([d[1]],[f]);a=(a.cacheable?a.fragment.cloneNode(true):a.fragment).childNodes}return c.merge(this,a)}else{if(b=s.getElementById(d[2])){if(b.id!==d[2])return T.find(a);this.length=1;this[0]=b}this.context=s;this.selector=a;return this}else if(!b&&/^\w+$/.test(a)){this.selector=a;this.context=s;a=s.getElementsByTagName(a);return c.merge(this,
-a)}else return!b||b.jquery?(b||T).find(a):c(b).find(a);else if(c.isFunction(a))return T.ready(a);if(a.selector!==w){this.selector=a.selector;this.context=a.context}return c.makeArray(a,this)},selector:"",jquery:"1.4.2",length:0,size:function(){return this.length},toArray:function(){return R.call(this,0)},get:function(a){return a==null?this.toArray():a<0?this.slice(a)[0]:this[a]},pushStack:function(a,b,d){var f=c();c.isArray(a)?ba.apply(f,a):c.merge(f,a);f.prevObject=this;f.context=this.context;if(b===
-"find")f.selector=this.selector+(this.selector?" ":"")+d;else if(b)f.selector=this.selector+"."+b+"("+d+")";return f},each:function(a,b){return c.each(this,a,b)},ready:function(a){c.bindReady();if(c.isReady)a.call(s,c);else Q&&Q.push(a);return this},eq:function(a){return a===-1?this.slice(a):this.slice(a,+a+1)},first:function(){return this.eq(0)},last:function(){return this.eq(-1)},slice:function(){return this.pushStack(R.apply(this,arguments),"slice",R.call(arguments).join(","))},map:function(a){return this.pushStack(c.map(this,
-function(b,d){return a.call(b,d,b)}))},end:function(){return this.prevObject||c(null)},push:ba,sort:[].sort,splice:[].splice};c.fn.init.prototype=c.fn;c.extend=c.fn.extend=function(){var a=arguments[0]||{},b=1,d=arguments.length,f=false,e,j,i,o;if(typeof a==="boolean"){f=a;a=arguments[1]||{};b=2}if(typeof a!=="object"&&!c.isFunction(a))a={};if(d===b){a=this;--b}for(;ba ";
-var e=d.getElementsByTagName("*"),j=d.getElementsByTagName("a")[0];if(!(!e||!e.length||!j)){c.support={leadingWhitespace:d.firstChild.nodeType===3,tbody:!d.getElementsByTagName("tbody").length,htmlSerialize:!!d.getElementsByTagName("link").length,style:/red/.test(j.getAttribute("style")),hrefNormalized:j.getAttribute("href")==="/a",opacity:/^0.55$/.test(j.style.opacity),cssFloat:!!j.style.cssFloat,checkOn:d.getElementsByTagName("input")[0].value==="on",optSelected:s.createElement("select").appendChild(s.createElement("option")).selected,
-parentNode:d.removeChild(d.appendChild(s.createElement("div"))).parentNode===null,deleteExpando:true,checkClone:false,scriptEval:false,noCloneEvent:true,boxModel:null};b.type="text/javascript";try{b.appendChild(s.createTextNode("window."+f+"=1;"))}catch(i){}a.insertBefore(b,a.firstChild);if(A[f]){c.support.scriptEval=true;delete A[f]}try{delete b.test}catch(o){c.support.deleteExpando=false}a.removeChild(b);if(d.attachEvent&&d.fireEvent){d.attachEvent("onclick",function k(){c.support.noCloneEvent=
-false;d.detachEvent("onclick",k)});d.cloneNode(true).fireEvent("onclick")}d=s.createElement("div");d.innerHTML=" ";a=s.createDocumentFragment();a.appendChild(d.firstChild);c.support.checkClone=a.cloneNode(true).cloneNode(true).lastChild.checked;c(function(){var k=s.createElement("div");k.style.width=k.style.paddingLeft="1px";s.body.appendChild(k);c.boxModel=c.support.boxModel=k.offsetWidth===2;s.body.removeChild(k).style.display="none"});a=function(k){var n=
-s.createElement("div");k="on"+k;var r=k in n;if(!r){n.setAttribute(k,"return;");r=typeof n[k]==="function"}return r};c.support.submitBubbles=a("submit");c.support.changeBubbles=a("change");a=b=d=e=j=null}})();c.props={"for":"htmlFor","class":"className",readonly:"readOnly",maxlength:"maxLength",cellspacing:"cellSpacing",rowspan:"rowSpan",colspan:"colSpan",tabindex:"tabIndex",usemap:"useMap",frameborder:"frameBorder"};var G="jQuery"+J(),Ya=0,za={};c.extend({cache:{},expando:G,noData:{embed:true,object:true,
-applet:true},data:function(a,b,d){if(!(a.nodeName&&c.noData[a.nodeName.toLowerCase()])){a=a==A?za:a;var f=a[G],e=c.cache;if(!f&&typeof b==="string"&&d===w)return null;f||(f=++Ya);if(typeof b==="object"){a[G]=f;e[f]=c.extend(true,{},b)}else if(!e[f]){a[G]=f;e[f]={}}a=e[f];if(d!==w)a[b]=d;return typeof b==="string"?a[b]:a}},removeData:function(a,b){if(!(a.nodeName&&c.noData[a.nodeName.toLowerCase()])){a=a==A?za:a;var d=a[G],f=c.cache,e=f[d];if(b){if(e){delete e[b];c.isEmptyObject(e)&&c.removeData(a)}}else{if(c.support.deleteExpando)delete a[c.expando];
-else a.removeAttribute&&a.removeAttribute(c.expando);delete f[d]}}}});c.fn.extend({data:function(a,b){if(typeof a==="undefined"&&this.length)return c.data(this[0]);else if(typeof a==="object")return this.each(function(){c.data(this,a)});var d=a.split(".");d[1]=d[1]?"."+d[1]:"";if(b===w){var f=this.triggerHandler("getData"+d[1]+"!",[d[0]]);if(f===w&&this.length)f=c.data(this[0],a);return f===w&&d[1]?this.data(d[0]):f}else return this.trigger("setData"+d[1]+"!",[d[0],b]).each(function(){c.data(this,
-a,b)})},removeData:function(a){return this.each(function(){c.removeData(this,a)})}});c.extend({queue:function(a,b,d){if(a){b=(b||"fx")+"queue";var f=c.data(a,b);if(!d)return f||[];if(!f||c.isArray(d))f=c.data(a,b,c.makeArray(d));else f.push(d);return f}},dequeue:function(a,b){b=b||"fx";var d=c.queue(a,b),f=d.shift();if(f==="inprogress")f=d.shift();if(f){b==="fx"&&d.unshift("inprogress");f.call(a,function(){c.dequeue(a,b)})}}});c.fn.extend({queue:function(a,b){if(typeof a!=="string"){b=a;a="fx"}if(b===
-w)return c.queue(this[0],a);return this.each(function(){var d=c.queue(this,a,b);a==="fx"&&d[0]!=="inprogress"&&c.dequeue(this,a)})},dequeue:function(a){return this.each(function(){c.dequeue(this,a)})},delay:function(a,b){a=c.fx?c.fx.speeds[a]||a:a;b=b||"fx";return this.queue(b,function(){var d=this;setTimeout(function(){c.dequeue(d,b)},a)})},clearQueue:function(a){return this.queue(a||"fx",[])}});var Aa=/[\n\t]/g,ca=/\s+/,Za=/\r/g,$a=/href|src|style/,ab=/(button|input)/i,bb=/(button|input|object|select|textarea)/i,
-cb=/^(a|area)$/i,Ba=/radio|checkbox/;c.fn.extend({attr:function(a,b){return X(this,a,b,true,c.attr)},removeAttr:function(a){return this.each(function(){c.attr(this,a,"");this.nodeType===1&&this.removeAttribute(a)})},addClass:function(a){if(c.isFunction(a))return this.each(function(n){var r=c(this);r.addClass(a.call(this,n,r.attr("class")))});if(a&&typeof a==="string")for(var b=(a||"").split(ca),d=0,f=this.length;d-1)return true;return false},val:function(a){if(a===w){var b=this[0];if(b){if(c.nodeName(b,"option"))return(b.attributes.value||{}).specified?b.value:b.text;if(c.nodeName(b,"select")){var d=b.selectedIndex,f=[],e=b.options;b=b.type==="select-one";if(d<0)return null;var j=b?d:0;for(d=b?d+1:e.length;j=0;else if(c.nodeName(this,"select")){var u=c.makeArray(r);c("option",this).each(function(){this.selected=
-c.inArray(c(this).val(),u)>=0});if(!u.length)this.selectedIndex=-1}else this.value=r}})}});c.extend({attrFn:{val:true,css:true,html:true,text:true,data:true,width:true,height:true,offset:true},attr:function(a,b,d,f){if(!a||a.nodeType===3||a.nodeType===8)return w;if(f&&b in c.attrFn)return c(a)[b](d);f=a.nodeType!==1||!c.isXMLDoc(a);var e=d!==w;b=f&&c.props[b]||b;if(a.nodeType===1){var j=$a.test(b);if(b in a&&f&&!j){if(e){b==="type"&&ab.test(a.nodeName)&&a.parentNode&&c.error("type property can't be changed");
-a[b]=d}if(c.nodeName(a,"form")&&a.getAttributeNode(b))return a.getAttributeNode(b).nodeValue;if(b==="tabIndex")return(b=a.getAttributeNode("tabIndex"))&&b.specified?b.value:bb.test(a.nodeName)||cb.test(a.nodeName)&&a.href?0:w;return a[b]}if(!c.support.style&&f&&b==="style"){if(e)a.style.cssText=""+d;return a.style.cssText}e&&a.setAttribute(b,""+d);a=!c.support.hrefNormalized&&f&&j?a.getAttribute(b,2):a.getAttribute(b);return a===null?w:a}return c.style(a,b,d)}});var O=/\.(.*)$/,db=function(a){return a.replace(/[^\w\s\.\|`]/g,
-function(b){return"\\"+b})};c.event={add:function(a,b,d,f){if(!(a.nodeType===3||a.nodeType===8)){if(a.setInterval&&a!==A&&!a.frameElement)a=A;var e,j;if(d.handler){e=d;d=e.handler}if(!d.guid)d.guid=c.guid++;if(j=c.data(a)){var i=j.events=j.events||{},o=j.handle;if(!o)j.handle=o=function(){return typeof c!=="undefined"&&!c.event.triggered?c.event.handle.apply(o.elem,arguments):w};o.elem=a;b=b.split(" ");for(var k,n=0,r;k=b[n++];){j=e?c.extend({},e):{handler:d,data:f};if(k.indexOf(".")>-1){r=k.split(".");
-k=r.shift();j.namespace=r.slice(0).sort().join(".")}else{r=[];j.namespace=""}j.type=k;j.guid=d.guid;var u=i[k],z=c.event.special[k]||{};if(!u){u=i[k]=[];if(!z.setup||z.setup.call(a,f,r,o)===false)if(a.addEventListener)a.addEventListener(k,o,false);else a.attachEvent&&a.attachEvent("on"+k,o)}if(z.add){z.add.call(a,j);if(!j.handler.guid)j.handler.guid=d.guid}u.push(j);c.event.global[k]=true}a=null}}},global:{},remove:function(a,b,d,f){if(!(a.nodeType===3||a.nodeType===8)){var e,j=0,i,o,k,n,r,u,z=c.data(a),
-C=z&&z.events;if(z&&C){if(b&&b.type){d=b.handler;b=b.type}if(!b||typeof b==="string"&&b.charAt(0)==="."){b=b||"";for(e in C)c.event.remove(a,e+b)}else{for(b=b.split(" ");e=b[j++];){n=e;i=e.indexOf(".")<0;o=[];if(!i){o=e.split(".");e=o.shift();k=new RegExp("(^|\\.)"+c.map(o.slice(0).sort(),db).join("\\.(?:.*\\.)?")+"(\\.|$)")}if(r=C[e])if(d){n=c.event.special[e]||{};for(B=f||0;B=0){a.type=
-e=e.slice(0,-1);a.exclusive=true}if(!d){a.stopPropagation();c.event.global[e]&&c.each(c.cache,function(){this.events&&this.events[e]&&c.event.trigger(a,b,this.handle.elem)})}if(!d||d.nodeType===3||d.nodeType===8)return w;a.result=w;a.target=d;b=c.makeArray(b);b.unshift(a)}a.currentTarget=d;(f=c.data(d,"handle"))&&f.apply(d,b);f=d.parentNode||d.ownerDocument;try{if(!(d&&d.nodeName&&c.noData[d.nodeName.toLowerCase()]))if(d["on"+e]&&d["on"+e].apply(d,b)===false)a.result=false}catch(j){}if(!a.isPropagationStopped()&&
-f)c.event.trigger(a,b,f,true);else if(!a.isDefaultPrevented()){f=a.target;var i,o=c.nodeName(f,"a")&&e==="click",k=c.event.special[e]||{};if((!k._default||k._default.call(d,a)===false)&&!o&&!(f&&f.nodeName&&c.noData[f.nodeName.toLowerCase()])){try{if(f[e]){if(i=f["on"+e])f["on"+e]=null;c.event.triggered=true;f[e]()}}catch(n){}if(i)f["on"+e]=i;c.event.triggered=false}}},handle:function(a){var b,d,f,e;a=arguments[0]=c.event.fix(a||A.event);a.currentTarget=this;b=a.type.indexOf(".")<0&&!a.exclusive;
-if(!b){d=a.type.split(".");a.type=d.shift();f=new RegExp("(^|\\.)"+d.slice(0).sort().join("\\.(?:.*\\.)?")+"(\\.|$)")}e=c.data(this,"events");d=e[a.type];if(e&&d){d=d.slice(0);e=0;for(var j=d.length;e-1?c.map(a.options,function(f){return f.selected}).join("-"):"";else if(a.nodeName.toLowerCase()==="select")d=a.selectedIndex;return d},fa=function(a,b){var d=a.target,f,e;if(!(!da.test(d.nodeName)||d.readOnly)){f=c.data(d,"_change_data");e=Fa(d);if(a.type!=="focusout"||d.type!=="radio")c.data(d,"_change_data",
-e);if(!(f===w||e===f))if(f!=null||e){a.type="change";return c.event.trigger(a,b,d)}}};c.event.special.change={filters:{focusout:fa,click:function(a){var b=a.target,d=b.type;if(d==="radio"||d==="checkbox"||b.nodeName.toLowerCase()==="select")return fa.call(this,a)},keydown:function(a){var b=a.target,d=b.type;if(a.keyCode===13&&b.nodeName.toLowerCase()!=="textarea"||a.keyCode===32&&(d==="checkbox"||d==="radio")||d==="select-multiple")return fa.call(this,a)},beforeactivate:function(a){a=a.target;c.data(a,
-"_change_data",Fa(a))}},setup:function(){if(this.type==="file")return false;for(var a in ea)c.event.add(this,a+".specialChange",ea[a]);return da.test(this.nodeName)},teardown:function(){c.event.remove(this,".specialChange");return da.test(this.nodeName)}};ea=c.event.special.change.filters}s.addEventListener&&c.each({focus:"focusin",blur:"focusout"},function(a,b){function d(f){f=c.event.fix(f);f.type=b;return c.event.handle.call(this,f)}c.event.special[b]={setup:function(){this.addEventListener(a,
-d,true)},teardown:function(){this.removeEventListener(a,d,true)}}});c.each(["bind","one"],function(a,b){c.fn[b]=function(d,f,e){if(typeof d==="object"){for(var j in d)this[b](j,f,d[j],e);return this}if(c.isFunction(f)){e=f;f=w}var i=b==="one"?c.proxy(e,function(k){c(this).unbind(k,i);return e.apply(this,arguments)}):e;if(d==="unload"&&b!=="one")this.one(d,f,e);else{j=0;for(var o=this.length;j0){y=t;break}}t=t[g]}m[q]=y}}}var f=/((?:\((?:\([^()]+\)|[^()]+)+\)|\[(?:\[[^[\]]*\]|['"][^'"]*['"]|[^[\]'"]+)+\]|\\.|[^ >+~,(\[\\]+)+|[>+~])(\s*,\s*)?((?:.|\r|\n)*)/g,
-e=0,j=Object.prototype.toString,i=false,o=true;[0,0].sort(function(){o=false;return 0});var k=function(g,h,l,m){l=l||[];var q=h=h||s;if(h.nodeType!==1&&h.nodeType!==9)return[];if(!g||typeof g!=="string")return l;for(var p=[],v,t,y,S,H=true,M=x(h),I=g;(f.exec(""),v=f.exec(I))!==null;){I=v[3];p.push(v[1]);if(v[2]){S=v[3];break}}if(p.length>1&&r.exec(g))if(p.length===2&&n.relative[p[0]])t=ga(p[0]+p[1],h);else for(t=n.relative[p[0]]?[h]:k(p.shift(),h);p.length;){g=p.shift();if(n.relative[g])g+=p.shift();
-t=ga(g,t)}else{if(!m&&p.length>1&&h.nodeType===9&&!M&&n.match.ID.test(p[0])&&!n.match.ID.test(p[p.length-1])){v=k.find(p.shift(),h,M);h=v.expr?k.filter(v.expr,v.set)[0]:v.set[0]}if(h){v=m?{expr:p.pop(),set:z(m)}:k.find(p.pop(),p.length===1&&(p[0]==="~"||p[0]==="+")&&h.parentNode?h.parentNode:h,M);t=v.expr?k.filter(v.expr,v.set):v.set;if(p.length>0)y=z(t);else H=false;for(;p.length;){var D=p.pop();v=D;if(n.relative[D])v=p.pop();else D="";if(v==null)v=h;n.relative[D](y,v,M)}}else y=[]}y||(y=t);y||k.error(D||
-g);if(j.call(y)==="[object Array]")if(H)if(h&&h.nodeType===1)for(g=0;y[g]!=null;g++){if(y[g]&&(y[g]===true||y[g].nodeType===1&&E(h,y[g])))l.push(t[g])}else for(g=0;y[g]!=null;g++)y[g]&&y[g].nodeType===1&&l.push(t[g]);else l.push.apply(l,y);else z(y,l);if(S){k(S,q,l,m);k.uniqueSort(l)}return l};k.uniqueSort=function(g){if(B){i=o;g.sort(B);if(i)for(var h=1;h":function(g,h){var l=typeof h==="string";if(l&&!/\W/.test(h)){h=h.toLowerCase();for(var m=0,q=g.length;m=0))l||m.push(v);else if(l)h[p]=false;return false},ID:function(g){return g[1].replace(/\\/g,"")},TAG:function(g){return g[1].toLowerCase()},
-CHILD:function(g){if(g[1]==="nth"){var h=/(-?)(\d*)n((?:\+|-)?\d*)/.exec(g[2]==="even"&&"2n"||g[2]==="odd"&&"2n+1"||!/\D/.test(g[2])&&"0n+"+g[2]||g[2]);g[2]=h[1]+(h[2]||1)-0;g[3]=h[3]-0}g[0]=e++;return g},ATTR:function(g,h,l,m,q,p){h=g[1].replace(/\\/g,"");if(!p&&n.attrMap[h])g[1]=n.attrMap[h];if(g[2]==="~=")g[4]=" "+g[4]+" ";return g},PSEUDO:function(g,h,l,m,q){if(g[1]==="not")if((f.exec(g[3])||"").length>1||/^\w/.test(g[3]))g[3]=k(g[3],null,null,h);else{g=k.filter(g[3],h,l,true^q);l||m.push.apply(m,
-g);return false}else if(n.match.POS.test(g[0])||n.match.CHILD.test(g[0]))return true;return g},POS:function(g){g.unshift(true);return g}},filters:{enabled:function(g){return g.disabled===false&&g.type!=="hidden"},disabled:function(g){return g.disabled===true},checked:function(g){return g.checked===true},selected:function(g){return g.selected===true},parent:function(g){return!!g.firstChild},empty:function(g){return!g.firstChild},has:function(g,h,l){return!!k(l[3],g).length},header:function(g){return/h\d/i.test(g.nodeName)},
-text:function(g){return"text"===g.type},radio:function(g){return"radio"===g.type},checkbox:function(g){return"checkbox"===g.type},file:function(g){return"file"===g.type},password:function(g){return"password"===g.type},submit:function(g){return"submit"===g.type},image:function(g){return"image"===g.type},reset:function(g){return"reset"===g.type},button:function(g){return"button"===g.type||g.nodeName.toLowerCase()==="button"},input:function(g){return/input|select|textarea|button/i.test(g.nodeName)}},
-setFilters:{first:function(g,h){return h===0},last:function(g,h,l,m){return h===m.length-1},even:function(g,h){return h%2===0},odd:function(g,h){return h%2===1},lt:function(g,h,l){return hl[3]-0},nth:function(g,h,l){return l[3]-0===h},eq:function(g,h,l){return l[3]-0===h}},filter:{PSEUDO:function(g,h,l,m){var q=h[1],p=n.filters[q];if(p)return p(g,l,h,m);else if(q==="contains")return(g.textContent||g.innerText||a([g])||"").indexOf(h[3])>=0;else if(q==="not"){h=
-h[3];l=0;for(m=h.length;l=0}},ID:function(g,h){return g.nodeType===1&&g.getAttribute("id")===h},TAG:function(g,h){return h==="*"&&g.nodeType===1||g.nodeName.toLowerCase()===h},CLASS:function(g,h){return(" "+(g.className||g.getAttribute("class"))+" ").indexOf(h)>-1},ATTR:function(g,h){var l=h[1];g=n.attrHandle[l]?n.attrHandle[l](g):g[l]!=null?g[l]:g.getAttribute(l);l=g+"";var m=h[2];h=h[4];return g==null?m==="!=":m===
-"="?l===h:m==="*="?l.indexOf(h)>=0:m==="~="?(" "+l+" ").indexOf(h)>=0:!h?l&&g!==false:m==="!="?l!==h:m==="^="?l.indexOf(h)===0:m==="$="?l.substr(l.length-h.length)===h:m==="|="?l===h||l.substr(0,h.length+1)===h+"-":false},POS:function(g,h,l,m){var q=n.setFilters[h[2]];if(q)return q(g,l,h,m)}}},r=n.match.POS;for(var u in n.match){n.match[u]=new RegExp(n.match[u].source+/(?![^\[]*\])(?![^\(]*\))/.source);n.leftMatch[u]=new RegExp(/(^(?:.|\r|\n)*?)/.source+n.match[u].source.replace(/\\(\d+)/g,function(g,
-h){return"\\"+(h-0+1)}))}var z=function(g,h){g=Array.prototype.slice.call(g,0);if(h){h.push.apply(h,g);return h}return g};try{Array.prototype.slice.call(s.documentElement.childNodes,0)}catch(C){z=function(g,h){h=h||[];if(j.call(g)==="[object Array]")Array.prototype.push.apply(h,g);else if(typeof g.length==="number")for(var l=0,m=g.length;l ";var l=s.documentElement;l.insertBefore(g,l.firstChild);if(s.getElementById(h)){n.find.ID=function(m,q,p){if(typeof q.getElementById!=="undefined"&&!p)return(q=q.getElementById(m[1]))?q.id===m[1]||typeof q.getAttributeNode!=="undefined"&&
-q.getAttributeNode("id").nodeValue===m[1]?[q]:w:[]};n.filter.ID=function(m,q){var p=typeof m.getAttributeNode!=="undefined"&&m.getAttributeNode("id");return m.nodeType===1&&p&&p.nodeValue===q}}l.removeChild(g);l=g=null})();(function(){var g=s.createElement("div");g.appendChild(s.createComment(""));if(g.getElementsByTagName("*").length>0)n.find.TAG=function(h,l){l=l.getElementsByTagName(h[1]);if(h[1]==="*"){h=[];for(var m=0;l[m];m++)l[m].nodeType===1&&h.push(l[m]);l=h}return l};g.innerHTML=" ";
-if(g.firstChild&&typeof g.firstChild.getAttribute!=="undefined"&&g.firstChild.getAttribute("href")!=="#")n.attrHandle.href=function(h){return h.getAttribute("href",2)};g=null})();s.querySelectorAll&&function(){var g=k,h=s.createElement("div");h.innerHTML="
";if(!(h.querySelectorAll&&h.querySelectorAll(".TEST").length===0)){k=function(m,q,p,v){q=q||s;if(!v&&q.nodeType===9&&!x(q))try{return z(q.querySelectorAll(m),p)}catch(t){}return g(m,q,p,v)};for(var l in g)k[l]=g[l];h=null}}();
-(function(){var g=s.createElement("div");g.innerHTML="
";if(!(!g.getElementsByClassName||g.getElementsByClassName("e").length===0)){g.lastChild.className="e";if(g.getElementsByClassName("e").length!==1){n.order.splice(1,0,"CLASS");n.find.CLASS=function(h,l,m){if(typeof l.getElementsByClassName!=="undefined"&&!m)return l.getElementsByClassName(h[1])};g=null}}})();var E=s.compareDocumentPosition?function(g,h){return!!(g.compareDocumentPosition(h)&16)}:
-function(g,h){return g!==h&&(g.contains?g.contains(h):true)},x=function(g){return(g=(g?g.ownerDocument||g:0).documentElement)?g.nodeName!=="HTML":false},ga=function(g,h){var l=[],m="",q;for(h=h.nodeType?[h]:h;q=n.match.PSEUDO.exec(g);){m+=q[0];g=g.replace(n.match.PSEUDO,"")}g=n.relative[g]?g+"*":g;q=0;for(var p=h.length;q=0===d})};c.fn.extend({find:function(a){for(var b=this.pushStack("","find",a),d=0,f=0,e=this.length;f0)for(var j=d;j0},closest:function(a,b){if(c.isArray(a)){var d=[],f=this[0],e,j=
-{},i;if(f&&a.length){e=0;for(var o=a.length;e-1:c(f).is(e)){d.push({selector:i,elem:f});delete j[i]}}f=f.parentNode}}return d}var k=c.expr.match.POS.test(a)?c(a,b||this.context):null;return this.map(function(n,r){for(;r&&r.ownerDocument&&r!==b;){if(k?k.index(r)>-1:c(r).is(a))return r;r=r.parentNode}return null})},index:function(a){if(!a||typeof a===
-"string")return c.inArray(this[0],a?c(a):this.parent().children());return c.inArray(a.jquery?a[0]:a,this)},add:function(a,b){a=typeof a==="string"?c(a,b||this.context):c.makeArray(a);b=c.merge(this.get(),a);return this.pushStack(qa(a[0])||qa(b[0])?b:c.unique(b))},andSelf:function(){return this.add(this.prevObject)}});c.each({parent:function(a){return(a=a.parentNode)&&a.nodeType!==11?a:null},parents:function(a){return c.dir(a,"parentNode")},parentsUntil:function(a,b,d){return c.dir(a,"parentNode",
-d)},next:function(a){return c.nth(a,2,"nextSibling")},prev:function(a){return c.nth(a,2,"previousSibling")},nextAll:function(a){return c.dir(a,"nextSibling")},prevAll:function(a){return c.dir(a,"previousSibling")},nextUntil:function(a,b,d){return c.dir(a,"nextSibling",d)},prevUntil:function(a,b,d){return c.dir(a,"previousSibling",d)},siblings:function(a){return c.sibling(a.parentNode.firstChild,a)},children:function(a){return c.sibling(a.firstChild)},contents:function(a){return c.nodeName(a,"iframe")?
-a.contentDocument||a.contentWindow.document:c.makeArray(a.childNodes)}},function(a,b){c.fn[a]=function(d,f){var e=c.map(this,b,d);eb.test(a)||(f=d);if(f&&typeof f==="string")e=c.filter(f,e);e=this.length>1?c.unique(e):e;if((this.length>1||gb.test(f))&&fb.test(a))e=e.reverse();return this.pushStack(e,a,R.call(arguments).join(","))}});c.extend({filter:function(a,b,d){if(d)a=":not("+a+")";return c.find.matches(a,b)},dir:function(a,b,d){var f=[];for(a=a[b];a&&a.nodeType!==9&&(d===w||a.nodeType!==1||!c(a).is(d));){a.nodeType===
-1&&f.push(a);a=a[b]}return f},nth:function(a,b,d){b=b||1;for(var f=0;a;a=a[d])if(a.nodeType===1&&++f===b)break;return a},sibling:function(a,b){for(var d=[];a;a=a.nextSibling)a.nodeType===1&&a!==b&&d.push(a);return d}});var Ja=/ jQuery\d+="(?:\d+|null)"/g,V=/^\s+/,Ka=/(<([\w:]+)[^>]*?)\/>/g,hb=/^(?:area|br|col|embed|hr|img|input|link|meta|param)$/i,La=/<([\w:]+)/,ib=/"+d+">"},F={option:[1,""," "],legend:[1,""," "],thead:[1,""],tr:[2,""],td:[3,""],col:[2,""],area:[1,""," "],_default:[0,"",""]};F.optgroup=F.option;F.tbody=F.tfoot=F.colgroup=F.caption=F.thead;F.th=F.td;if(!c.support.htmlSerialize)F._default=[1,"div","
"];c.fn.extend({text:function(a){if(c.isFunction(a))return this.each(function(b){var d=
-c(this);d.text(a.call(this,b,d.text()))});if(typeof a!=="object"&&a!==w)return this.empty().append((this[0]&&this[0].ownerDocument||s).createTextNode(a));return c.text(this)},wrapAll:function(a){if(c.isFunction(a))return this.each(function(d){c(this).wrapAll(a.call(this,d))});if(this[0]){var b=c(a,this[0].ownerDocument).eq(0).clone(true);this[0].parentNode&&b.insertBefore(this[0]);b.map(function(){for(var d=this;d.firstChild&&d.firstChild.nodeType===1;)d=d.firstChild;return d}).append(this)}return this},
-wrapInner:function(a){if(c.isFunction(a))return this.each(function(b){c(this).wrapInner(a.call(this,b))});return this.each(function(){var b=c(this),d=b.contents();d.length?d.wrapAll(a):b.append(a)})},wrap:function(a){return this.each(function(){c(this).wrapAll(a)})},unwrap:function(){return this.parent().each(function(){c.nodeName(this,"body")||c(this).replaceWith(this.childNodes)}).end()},append:function(){return this.domManip(arguments,true,function(a){this.nodeType===1&&this.appendChild(a)})},
-prepend:function(){return this.domManip(arguments,true,function(a){this.nodeType===1&&this.insertBefore(a,this.firstChild)})},before:function(){if(this[0]&&this[0].parentNode)return this.domManip(arguments,false,function(b){this.parentNode.insertBefore(b,this)});else if(arguments.length){var a=c(arguments[0]);a.push.apply(a,this.toArray());return this.pushStack(a,"before",arguments)}},after:function(){if(this[0]&&this[0].parentNode)return this.domManip(arguments,false,function(b){this.parentNode.insertBefore(b,
-this.nextSibling)});else if(arguments.length){var a=this.pushStack(this,"after",arguments);a.push.apply(a,c(arguments[0]).toArray());return a}},remove:function(a,b){for(var d=0,f;(f=this[d])!=null;d++)if(!a||c.filter(a,[f]).length){if(!b&&f.nodeType===1){c.cleanData(f.getElementsByTagName("*"));c.cleanData([f])}f.parentNode&&f.parentNode.removeChild(f)}return this},empty:function(){for(var a=0,b;(b=this[a])!=null;a++)for(b.nodeType===1&&c.cleanData(b.getElementsByTagName("*"));b.firstChild;)b.removeChild(b.firstChild);
-return this},clone:function(a){var b=this.map(function(){if(!c.support.noCloneEvent&&!c.isXMLDoc(this)){var d=this.outerHTML,f=this.ownerDocument;if(!d){d=f.createElement("div");d.appendChild(this.cloneNode(true));d=d.innerHTML}return c.clean([d.replace(Ja,"").replace(/=([^="'>\s]+\/)>/g,'="$1">').replace(V,"")],f)[0]}else return this.cloneNode(true)});if(a===true){ra(this,b);ra(this.find("*"),b.find("*"))}return b},html:function(a){if(a===w)return this[0]&&this[0].nodeType===1?this[0].innerHTML.replace(Ja,
-""):null;else if(typeof a==="string"&&!ta.test(a)&&(c.support.leadingWhitespace||!V.test(a))&&!F[(La.exec(a)||["",""])[1].toLowerCase()]){a=a.replace(Ka,Ma);try{for(var b=0,d=this.length;b0||e.cacheable||this.length>1?k.cloneNode(true):k)}o.length&&c.each(o,Qa)}return this}});c.fragments={};c.each({appendTo:"append",prependTo:"prepend",insertBefore:"before",insertAfter:"after",replaceAll:"replaceWith"},function(a,b){c.fn[a]=function(d){var f=[];d=c(d);var e=this.length===1&&this[0].parentNode;if(e&&e.nodeType===11&&e.childNodes.length===1&&d.length===1){d[b](this[0]);
-return this}else{e=0;for(var j=d.length;e0?this.clone(true):this).get();c.fn[b].apply(c(d[e]),i);f=f.concat(i)}return this.pushStack(f,a,d.selector)}}});c.extend({clean:function(a,b,d,f){b=b||s;if(typeof b.createElement==="undefined")b=b.ownerDocument||b[0]&&b[0].ownerDocument||s;for(var e=[],j=0,i;(i=a[j])!=null;j++){if(typeof i==="number")i+="";if(i){if(typeof i==="string"&&!jb.test(i))i=b.createTextNode(i);else if(typeof i==="string"){i=i.replace(Ka,Ma);var o=(La.exec(i)||["",
-""])[1].toLowerCase(),k=F[o]||F._default,n=k[0],r=b.createElement("div");for(r.innerHTML=k[1]+i+k[2];n--;)r=r.lastChild;if(!c.support.tbody){n=ib.test(i);o=o==="table"&&!n?r.firstChild&&r.firstChild.childNodes:k[1]===""&&!n?r.childNodes:[];for(k=o.length-1;k>=0;--k)c.nodeName(o[k],"tbody")&&!o[k].childNodes.length&&o[k].parentNode.removeChild(o[k])}!c.support.leadingWhitespace&&V.test(i)&&r.insertBefore(b.createTextNode(V.exec(i)[0]),r.firstChild);i=r.childNodes}if(i.nodeType)e.push(i);else e=
-c.merge(e,i)}}if(d)for(j=0;e[j];j++)if(f&&c.nodeName(e[j],"script")&&(!e[j].type||e[j].type.toLowerCase()==="text/javascript"))f.push(e[j].parentNode?e[j].parentNode.removeChild(e[j]):e[j]);else{e[j].nodeType===1&&e.splice.apply(e,[j+1,0].concat(c.makeArray(e[j].getElementsByTagName("script"))));d.appendChild(e[j])}return e},cleanData:function(a){for(var b,d,f=c.cache,e=c.event.special,j=c.support.deleteExpando,i=0,o;(o=a[i])!=null;i++)if(d=o[c.expando]){b=f[d];if(b.events)for(var k in b.events)e[k]?
-c.event.remove(o,k):Ca(o,k,b.handle);if(j)delete o[c.expando];else o.removeAttribute&&o.removeAttribute(c.expando);delete f[d]}}});var kb=/z-?index|font-?weight|opacity|zoom|line-?height/i,Na=/alpha\([^)]*\)/,Oa=/opacity=([^)]*)/,ha=/float/i,ia=/-([a-z])/ig,lb=/([A-Z])/g,mb=/^-?\d+(?:px)?$/i,nb=/^-?\d/,ob={position:"absolute",visibility:"hidden",display:"block"},pb=["Left","Right"],qb=["Top","Bottom"],rb=s.defaultView&&s.defaultView.getComputedStyle,Pa=c.support.cssFloat?"cssFloat":"styleFloat",ja=
-function(a,b){return b.toUpperCase()};c.fn.css=function(a,b){return X(this,a,b,true,function(d,f,e){if(e===w)return c.curCSS(d,f);if(typeof e==="number"&&!kb.test(f))e+="px";c.style(d,f,e)})};c.extend({style:function(a,b,d){if(!a||a.nodeType===3||a.nodeType===8)return w;if((b==="width"||b==="height")&&parseFloat(d)<0)d=w;var f=a.style||a,e=d!==w;if(!c.support.opacity&&b==="opacity"){if(e){f.zoom=1;b=parseInt(d,10)+""==="NaN"?"":"alpha(opacity="+d*100+")";a=f.filter||c.curCSS(a,"filter")||"";f.filter=
-Na.test(a)?a.replace(Na,b):b}return f.filter&&f.filter.indexOf("opacity=")>=0?parseFloat(Oa.exec(f.filter)[1])/100+"":""}if(ha.test(b))b=Pa;b=b.replace(ia,ja);if(e)f[b]=d;return f[b]},css:function(a,b,d,f){if(b==="width"||b==="height"){var e,j=b==="width"?pb:qb;function i(){e=b==="width"?a.offsetWidth:a.offsetHeight;f!=="border"&&c.each(j,function(){f||(e-=parseFloat(c.curCSS(a,"padding"+this,true))||0);if(f==="margin")e+=parseFloat(c.curCSS(a,"margin"+this,true))||0;else e-=parseFloat(c.curCSS(a,
-"border"+this+"Width",true))||0})}a.offsetWidth!==0?i():c.swap(a,ob,i);return Math.max(0,Math.round(e))}return c.curCSS(a,b,d)},curCSS:function(a,b,d){var f,e=a.style;if(!c.support.opacity&&b==="opacity"&&a.currentStyle){f=Oa.test(a.currentStyle.filter||"")?parseFloat(RegExp.$1)/100+"":"";return f===""?"1":f}if(ha.test(b))b=Pa;if(!d&&e&&e[b])f=e[b];else if(rb){if(ha.test(b))b="float";b=b.replace(lb,"-$1").toLowerCase();e=a.ownerDocument.defaultView;if(!e)return null;if(a=e.getComputedStyle(a,null))f=
-a.getPropertyValue(b);if(b==="opacity"&&f==="")f="1"}else if(a.currentStyle){d=b.replace(ia,ja);f=a.currentStyle[b]||a.currentStyle[d];if(!mb.test(f)&&nb.test(f)){b=e.left;var j=a.runtimeStyle.left;a.runtimeStyle.left=a.currentStyle.left;e.left=d==="fontSize"?"1em":f||0;f=e.pixelLeft+"px";e.left=b;a.runtimeStyle.left=j}}return f},swap:function(a,b,d){var f={};for(var e in b){f[e]=a.style[e];a.style[e]=b[e]}d.call(a);for(e in b)a.style[e]=f[e]}});if(c.expr&&c.expr.filters){c.expr.filters.hidden=function(a){var b=
-a.offsetWidth,d=a.offsetHeight,f=a.nodeName.toLowerCase()==="tr";return b===0&&d===0&&!f?true:b>0&&d>0&&!f?false:c.curCSS(a,"display")==="none"};c.expr.filters.visible=function(a){return!c.expr.filters.hidden(a)}}var sb=J(),tb=/
-
-
-
-
-
-
-
-
-