diff --git a/.gitignore b/.gitignore
index da442c20b7..3f6f5456a1 100644
--- a/.gitignore
+++ b/.gitignore
@@ -40,6 +40,7 @@
# /conf/
/conf/conf.xml
/conf/conf.xml.20*
+/conf/conf_personal.xml.20*
/conf/control_panel.xml
/conf/%gconf.xml
/conf/maps_data/*
diff --git a/Makefile b/Makefile
index 41b749dadc..b46b46b178 100644
--- a/Makefile
+++ b/Makefile
@@ -31,7 +31,7 @@ PAPARAZZI_SRC=$(shell pwd)
empty=
space=$(empty) $(empty)
ifneq ($(findstring $(space),$(PAPARAZZI_SRC)),)
- $(error No fucking spaces allowed in the current directory name)
+ $(error No spaces allowed in the current directory name)
endif
ifeq ($(PAPARAZZI_HOME),)
PAPARAZZI_HOME=$(PAPARAZZI_SRC)
@@ -113,7 +113,7 @@ update_google_version:
conf: conf/conf.xml conf/control_panel.xml conf/maps.xml
-conf/%.xml :conf/%.xml.example
+conf/%.xml :conf/%_example.xml
[ -L $@ ] || [ -f $@ ] || cp $< $@
@@ -248,6 +248,15 @@ paparazzi:
chmod a+x $@
+#
+# doxygen html documentation
+#
+dox:
+ $(Q)PAPARAZZI_HOME=$(PAPARAZZI_HOME) sw/tools/doxygen_gen/gen_modules_doc.py -pv
+ @echo "Generationg doxygen html documentation in doc/generated/html"
+ $(Q)( cat Doxyfile ; echo "PROJECT_NUMBER=$(./paparazzi_version)"; echo "QUIET=YES") | doxygen -
+ @echo "Done. Open doc/generated/html/index.html in your browser to view it."
+
#
# Cleaning
#
@@ -285,7 +294,7 @@ ab_clean:
replace_current_conf_xml:
test conf/conf.xml && mv conf/conf.xml conf/conf.xml.backup.$(BUILD_DATETIME)
- cp conf/tests_conf.xml conf/conf.xml
+ cp conf/conf_tests.xml conf/conf.xml
restore_conf_xml:
test conf/conf.xml.backup.$(BUILD_DATETIME) && mv conf/conf.xml.backup.$(BUILD_DATETIME) conf/conf.xml
@@ -296,7 +305,7 @@ run_tests:
test: all replace_current_conf_xml run_tests restore_conf_xml
-.PHONY: all print_build_version update_google_version ground_segment ground_segment.opt \
+.PHONY: all print_build_version update_google_version dox ground_segment ground_segment.opt \
subdirs $(SUBDIRS) conf ext libpprz multimon cockpit cockpit.opt tmtc tmtc.opt tools\
static sim_static lpctools commands \
clean cleanspaces ab_clean dist_clean distclean dist_clean_irreversible \
diff --git a/Makefile.ac b/Makefile.ac
index 90e2b5e3d0..9939768bcd 100644
--- a/Makefile.ac
+++ b/Makefile.ac
@@ -64,10 +64,19 @@ Q=@
#
ifeq ($(MAKECMDGOALS),all_ac_h)
-include $(MAKEFILE_AC)
+
+ifdef PERIODIC_FREQUENCY
+# telemetry and module periodic frequency default to PERIODIC_FREQUENCY
+TELEMETRY_FREQUENCY ?= $(PERIODIC_FREQUENCY)
+DEFAULT_MODULES_FREQUENCY = $(PERIODIC_FREQUENCY)
+else
+$(warning Info: PERIODIC_FREQUENCY not configured, defaulting to 60Hz for modules and telemetry)
+TELEMETRY_FREQUENCY ?= 60
+DEFAULT_MODULES_FREQUENCY = 60
+endif
+
endif
-# telemetry periodic frequency defaults to 60Hz
-TELEMETRY_FREQUENCY ?= 60
init:
@[ -d $(PAPARAZZI_HOME) ] || (echo "Copying config example in your $(PAPARAZZI_HOME) directory"; mkdir -p $(PAPARAZZI_HOME); cp -a conf $(PAPARAZZI_HOME); cp -a data $(PAPARAZZI_HOME); mkdir -p $(PAPARAZZI_HOME)/var/maps; mkdir -p $(PAPARAZZI_HOME)/var/include)
@@ -147,7 +156,7 @@ $(MODULES_H) : $(CONF)/$(AIRFRAME_XML) $(TOOLS)/gen_modules.out $(CONF)/modules/
$(Q)test -d $(AC_GENERATED) || mkdir -p $(AC_GENERATED)
@echo GENERATE $@
$(eval $@_TMP := $(shell $(MKTEMP)))
- $(Q)$(TOOLS)/gen_modules.out $(SETTINGS_MODULES) $< > $($@_TMP)
+ $(Q)$(TOOLS)/gen_modules.out $(SETTINGS_MODULES) $(DEFAULT_MODULES_FREQUENCY) $< > $($@_TMP)
$(Q)mv $($@_TMP) $@
$(Q)chmod a+r $@
diff --git a/README.md b/README.md
index 8400e0d98a..86ced4a95d 100644
--- a/README.md
+++ b/README.md
@@ -21,7 +21,6 @@ Debian users can use http://paparazzi.enac.fr/debian
- **paparazzi-dev** is the meta-package that depends on everything needed to compile and run the ground segment and the simulator.
- **paparazzi-arm-multilib** ARM cross-compiling toolchain for LPC21 and STM32 based boards.
-- **paparazzi-omap** toolchain for the optional Gumstix Overo module available on lisa/L.
- **paparazzi-jsbsim** is needed for using JSBSim as flight dynamic model for the simulator.
@@ -66,4 +65,4 @@ Uploading of the embedded software
Flight
------
-1. From the Paparazzi Center, select the flight session and ... do the same than in simulation !
\ No newline at end of file
+1. From the Paparazzi Center, select the flight session and ... do the same than in simulation !
diff --git a/conf/Makefile.omap b/conf/Makefile.omap
index 361ea419ed..e567d0c314 100644
--- a/conf/Makefile.omap
+++ b/conf/Makefile.omap
@@ -78,42 +78,45 @@ elf: $(OBJDIR)/$(TARGET).elf
# Program the device and start it.
load upload program: $(OBJDIR)/$(TARGET).elf
-# If it is not the SDK version, then kill program.elf
-ifneq ($(BOARD_TYPE), sdk)
- -echo "killall -9 program.elf" | telnet $(HOST)
-endif
-
# Kill the application
-echo "killall -9 $(TARGET).elf" | telnet $(HOST)
- # Upload the modules and start the application
+ # Make the target dir and edit the config
-{ \
- echo "mkdir -p $(TARGET_DIR)"; \
+ echo "mkdir -p $(TARGET_DIR)"; \
+ echo "if grep -q \"start_paparazzi *= \" /data/config.ini; then sed -i 's/\(start_paparazzi *= *\).*/\\\1$(ARDRONE2_START_PAPARAZZI)/g' /data/config.ini; else echo \"start_paparazzi = $(ARDRONE2_START_PAPARAZZI)\" >> /data/config.ini; fi"; \
+ echo "if grep -q \"wifi_mode *= \" /data/config.ini; then sed -i 's/\(wifi_mode *= *\).*/\\\1$(ARDRONE2_WIFI_MODE)/g' /data/config.ini; else echo \"wifi_mode = $(ARDRONE2_WIFI_MODE)\" >> /data/config.ini; fi"; \
+ echo "if grep -q \"ssid_single_player *= \" /data/config.ini; then sed -i 's/\(ssid_single_player *= *\).*/\\\1$(ARDRONE2_SSID)/g' /data/config.ini; else echo \"ssid_single_player = $(ARDRONE2_SSID)\" >> /data/config.ini; fi"; \
+ echo "if grep -q \"static_ip_address_base *= \" /data/config.ini; then sed -i 's/\(static_ip_address_base *= *\).*/\\\1$(ARDRONE2_IP_ADDRESS_BASE)/g' /data/config.ini; else echo \"static_ip_address_base = $(ARDRONE2_IP_ADDRESS_BASE)\" >> /data/config.ini; fi"; \
+ echo "if grep -q \"static_ip_address_probe *= \" /data/config.ini; then sed -i 's/\(static_ip_address_probe *= *\).*/\\\1$(ARDRONE2_IP_ADDRESS_PROBE)/g' /data/config.ini; else echo \"static_ip_address_probe = $(ARDRONE2_IP_ADDRESS_PROBE)\" >> /data/config.ini; fi"; \
} | telnet $(HOST)
# Upload the drivers and new application
{ \
echo "binary"; \
echo "put $(PAPARAZZI_SRC)/sw/ext/ardrone2_drivers/cdc-acm.ko /$(SUB_DIR)/cdc-acm.ko"; \
- echo "put $(PAPARAZZI_SRC)/sw/ext/ardrone2_drivers/usbserial.ko /$(SUB_DIR)/usbserial.ko"; \
- echo "put $(PAPARAZZI_SRC)/sw/ext/ardrone2_drivers/pl2303.ko /$(SUB_DIR)/pl2303.ko"; \
- echo "put $(PAPARAZZI_SRC)/sw/ext/ardrone2_drivers/ftdi-sio.ko /$(SUB_DIR)/ftdi-sio.ko"; \
- echo "put $(PAPARAZZI_SRC)/sw/ext/ardrone2_drivers/cp210x.ko /$(SUB_DIR)/cp210x.ko"; \
+ echo "put $(PAPARAZZI_SRC)/sw/ext/ardrone2_drivers/check_update.sh check_update.sh"; \
+ echo "put $(PAPARAZZI_SRC)/sw/ext/ardrone2_drivers/wifi_setup.sh wifi_setup.sh"; \
echo "put $(OBJDIR)/$(TARGET).elf /$(SUB_DIR)/$(TARGET).elf"; \
echo "quit"; \
} | ftp -n $(HOST)
# Upload the modules and start the application
- -{ \
- echo "cd $(TARGET_DIR)"; \
- echo "insmod cdc-acm.ko"; \
- echo "insmod usbserial.ko"; \
- echo "insmod cp210x.ko"; \
- echo "insmod pl2303.ko"; \
- echo "insmod ftdi-sio.ko"; \
- echo "chmod 777 $(TARGET).elf"; \
- echo "./$(TARGET).elf > /dev/null 2>&1 &"; \
+ -{ \
+ echo "mv /data/video/check_update.sh /bin/"; \
+ echo "mv /data/video/wifi_setup.sh /bin/"; \
+ echo "chmod 777 /bin/check_update.sh" \
+ echo "chmod 777 /bin/wifi_setup.sh" \
+ echo "insmod $(TARGET_DIR)/cdc-acm.ko"; \
+ echo "chmod 777 $(TARGET_DIR)/$(TARGET).elf"; \
+ echo "$(TARGET_DIR)/$(TARGET).elf > /dev/null 2>&1 &"; \
} | telnet $(HOST)
+
+ifeq ($(ARDRONE2_REBOOT),1)
+ -{ \
+ echo "reboot"; \
+ } | telnet $(HOST)
+endif
# Link: create ELF output file from object files.
diff --git a/conf/airframes/CDW/asctec_cdw.xml b/conf/airframes/CDW/asctec_cdw.xml
new file mode 100644
index 0000000000..830c176d2f
--- /dev/null
+++ b/conf/airframes/CDW/asctec_cdw.xml
@@ -0,0 +1,227 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/CDW/conf.xml b/conf/airframes/CDW/conf.xml
new file mode 100644
index 0000000000..c8ba362e02
--- /dev/null
+++ b/conf/airframes/CDW/conf.xml
@@ -0,0 +1,22 @@
+
+
+
+
diff --git a/conf/airframes/CDW/tricopter_cdw.xml b/conf/airframes/CDW/tricopter_cdw.xml
new file mode 100644
index 0000000000..86946f11f3
--- /dev/null
+++ b/conf/airframes/CDW/tricopter_cdw.xml
@@ -0,0 +1,216 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/ENAC/fixed-wing/quark1.xml b/conf/airframes/ENAC/fixed-wing/quark1.xml
new file mode 100644
index 0000000000..10dfc81fe2
--- /dev/null
+++ b/conf/airframes/ENAC/fixed-wing/quark1.xml
@@ -0,0 +1,219 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/ENAC/quadrotor/booz2_g1.xml b/conf/airframes/ENAC/quadrotor/booz2_g1.xml
index ae5f73a8eb..834132e720 100644
--- a/conf/airframes/ENAC/quadrotor/booz2_g1.xml
+++ b/conf/airframes/ENAC/quadrotor/booz2_g1.xml
@@ -260,7 +260,7 @@
diff --git a/conf/airframes/NoVa_L.xml b/conf/airframes/NoVa_L.xml
index affb520b20..aaf45e3e27 100644
--- a/conf/airframes/NoVa_L.xml
+++ b/conf/airframes/NoVa_L.xml
@@ -228,7 +228,7 @@
diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/182_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/182_calib.xml
new file mode 100644
index 0000000000..3a14a4338e
--- /dev/null
+++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/182_calib.xml
@@ -0,0 +1,18 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/184_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/184_calib.xml
new file mode 100644
index 0000000000..383607ba9b
--- /dev/null
+++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/184_calib.xml
@@ -0,0 +1,20 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/186_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/186_calib.xml
new file mode 100644
index 0000000000..92af0c8aac
--- /dev/null
+++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/186_calib.xml
@@ -0,0 +1,23 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/188_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/188_calib.xml
new file mode 100644
index 0000000000..e3e8ef76bf
--- /dev/null
+++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/188_calib.xml
@@ -0,0 +1,23 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/189_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/189_calib.xml
new file mode 100644
index 0000000000..732f7e7065
--- /dev/null
+++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/189_calib.xml
@@ -0,0 +1,23 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/190_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/190_calib.xml
new file mode 100644
index 0000000000..d699f1a956
--- /dev/null
+++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/190_calib.xml
@@ -0,0 +1,18 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/191_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/191_calib.xml
new file mode 100644
index 0000000000..b5af215403
--- /dev/null
+++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/191_calib.xml
@@ -0,0 +1,18 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/192_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/192_calib.xml
new file mode 100644
index 0000000000..d6dd75562f
--- /dev/null
+++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/192_calib.xml
@@ -0,0 +1,18 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/193_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/193_calib.xml
new file mode 100644
index 0000000000..8fd05e01ae
--- /dev/null
+++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/193_calib.xml
@@ -0,0 +1,23 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/194_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/194_calib.xml
new file mode 100644
index 0000000000..14840c8c1e
--- /dev/null
+++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/194_calib.xml
@@ -0,0 +1,23 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/195_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/195_calib.xml
new file mode 100644
index 0000000000..36ef1433a2
--- /dev/null
+++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/195_calib.xml
@@ -0,0 +1,24 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/conf/airframes/TUDelft/IMAV2013/ARDrone/196_calib.xml b/conf/airframes/TUDelft/IMAV2013/ARDrone/196_calib.xml
new file mode 100644
index 0000000000..294856f650
--- /dev/null
+++ b/conf/airframes/TUDelft/IMAV2013/ARDrone/196_calib.xml
@@ -0,0 +1,23 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml b/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml
new file mode 100644
index 0000000000..cf2739449c
--- /dev/null
+++ b/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml
@@ -0,0 +1,206 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml b/conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml
new file mode 100644
index 0000000000..7a870f3b9c
--- /dev/null
+++ b/conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml
@@ -0,0 +1,223 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/TUDelft/IMAV2013/conf.xml b/conf/airframes/TUDelft/IMAV2013/conf.xml
new file mode 100644
index 0000000000..dff3725763
--- /dev/null
+++ b/conf/airframes/TUDelft/IMAV2013/conf.xml
@@ -0,0 +1,232 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/TUDelft/IMAV2013/mavrick_lisa_s.xml b/conf/airframes/TUDelft/IMAV2013/mavrick_lisa_s.xml
new file mode 100644
index 0000000000..1dc80175b2
--- /dev/null
+++ b/conf/airframes/TUDelft/IMAV2013/mavrick_lisa_s.xml
@@ -0,0 +1,181 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml b/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml
new file mode 100644
index 0000000000..064c01ae5a
--- /dev/null
+++ b/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml
@@ -0,0 +1,241 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml b/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml
new file mode 100644
index 0000000000..2ee2d7548e
--- /dev/null
+++ b/conf/airframes/TUDelft/IMAV2013/walkera_V120D02S.xml
@@ -0,0 +1,196 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml b/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml
new file mode 100644
index 0000000000..4e7dda0a2e
--- /dev/null
+++ b/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml
@@ -0,0 +1,201 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml b/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml
index 557d03acb9..ee9e0848ac 100644
--- a/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml
+++ b/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml
@@ -30,7 +30,7 @@
-
+
@@ -253,7 +253,7 @@
diff --git a/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml b/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml
index bdafdab212..06f2f97307 100644
--- a/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml
+++ b/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml
@@ -30,7 +30,7 @@
-
+
@@ -253,7 +253,7 @@
diff --git a/conf/airframes/ardrone2_raw.xml b/conf/airframes/ardrone2_raw.xml
index 29a86dda84..99265a1638 100644
--- a/conf/airframes/ardrone2_raw.xml
+++ b/conf/airframes/ardrone2_raw.xml
@@ -4,235 +4,222 @@
-
+
-
-
-
-
-
+
+
+
+
+
+
+
-
-
+
+
-
+
-
-
-
-
-
-
+
+
+
+
+
+
-
+
+
+
-
-
-
-
+
+
+
+
-
-
-
-
+
+
+
+
-
-
-
-
-
+
+
+
+
+
-
-
-
-
+
+
+
+
-
-
-
-
-
+
+
+
+
+
-
-
-
-
-
-
+
+
+
-
-
-
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
-
+
-
+
-
-
-
-
-
-
-
+
+
+
+
+
+
+
-
-
-
+
+
+
-
-
-
+
+
+
-
-
-
+
+
+
-
-
-
-
-
-
+
+
+
+
+
+
-
-
-
-
+
+
+
+
-
-
-
-
+
+
+
+
-
-
-
-
+
+
+
+
-
-
-
+
+
+
-
-
-
+
+
+
-
-
-
+
+
+
-
-
-
+
+
+
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
-
+
-
-
-
+
+
+
diff --git a/conf/airframes/ardrone2_sdk.xml b/conf/airframes/ardrone2_sdk.xml
index dfbcab0f90..73c21af476 100644
--- a/conf/airframes/ardrone2_sdk.xml
+++ b/conf/airframes/ardrone2_sdk.xml
@@ -6,6 +6,7 @@
+
@@ -84,9 +85,8 @@
-
-
+
+
diff --git a/conf/airframes/booz2_flixr.xml b/conf/airframes/booz2_flixr.xml
index 009f4312f1..a62ace8aee 100644
--- a/conf/airframes/booz2_flixr.xml
+++ b/conf/airframes/booz2_flixr.xml
@@ -19,7 +19,7 @@
-
+
@@ -232,7 +232,7 @@
diff --git a/conf/airframes/booz2_ppzuav.xml b/conf/airframes/booz2_ppzuav.xml
index 7e7320f043..af532c20f2 100644
--- a/conf/airframes/booz2_ppzuav.xml
+++ b/conf/airframes/booz2_ppzuav.xml
@@ -204,7 +204,7 @@
-
+
diff --git a/conf/airframes/esden/calib/ls01-default.xml b/conf/airframes/esden/calib/ls01-default.xml
new file mode 100644
index 0000000000..a7171f0158
--- /dev/null
+++ b/conf/airframes/esden/calib/ls01-default.xml
@@ -0,0 +1,34 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/esden/cocto_lm2a2.xml b/conf/airframes/esden/cocto_lm2a2.xml
index e035a7b5c7..609e7295bd 100644
--- a/conf/airframes/esden/cocto_lm2a2.xml
+++ b/conf/airframes/esden/cocto_lm2a2.xml
@@ -207,7 +207,7 @@ B2L -> CW
@@ -223,12 +223,10 @@ B2L -> CW
-
-
-
+
+
+
-
-
@@ -249,7 +247,7 @@ B2L -> CW
-
diff --git a/conf/airframes/esden/gain_scheduling_example.xml b/conf/airframes/esden/gain_scheduling_example.xml
index e9491e181a..02f574bd9f 100644
--- a/conf/airframes/esden/gain_scheduling_example.xml
+++ b/conf/airframes/esden/gain_scheduling_example.xml
@@ -189,7 +189,7 @@
@@ -233,7 +233,7 @@
-
diff --git a/conf/airframes/esden/hexy_ll11a2pwm.xml b/conf/airframes/esden/hexy_ll11a2pwm.xml
index 6ae1d2b368..f796f42c68 100644
--- a/conf/airframes/esden/hexy_ll11a2pwm.xml
+++ b/conf/airframes/esden/hexy_ll11a2pwm.xml
@@ -205,7 +205,7 @@
@@ -215,16 +215,14 @@
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
@@ -244,7 +242,7 @@
-
diff --git a/conf/airframes/esden/hexy_lm2a2pwm.xml b/conf/airframes/esden/hexy_lm2a2pwm.xml
index 00cf6e1bcc..73554f3209 100644
--- a/conf/airframes/esden/hexy_lm2a2pwm.xml
+++ b/conf/airframes/esden/hexy_lm2a2pwm.xml
@@ -166,7 +166,7 @@
@@ -178,12 +178,10 @@
-
-
-
+
+
+
-
-
@@ -205,7 +203,7 @@
-
diff --git a/conf/airframes/esden/lisa2_hex.xml b/conf/airframes/esden/lisa2_hex.xml
index 41d1d67104..b21dddffcb 100644
--- a/conf/airframes/esden/lisa2_hex.xml
+++ b/conf/airframes/esden/lisa2_hex.xml
@@ -177,7 +177,7 @@
@@ -195,8 +195,6 @@
-
-
@@ -220,7 +218,7 @@
-
diff --git a/conf/airframes/esden/qs_asp22.xml b/conf/airframes/esden/qs_asp22.xml
index 671309a1a6..c4ef62b938 100644
--- a/conf/airframes/esden/qs_asp22.xml
+++ b/conf/airframes/esden/qs_asp22.xml
@@ -200,7 +200,7 @@
@@ -244,7 +244,7 @@
-
diff --git a/conf/airframes/esden/quady_ll11a2pwm.xml b/conf/airframes/esden/quady_ll11a2pwm.xml
index 368bdb9a9e..ca80e5a5d2 100644
--- a/conf/airframes/esden/quady_ll11a2pwm.xml
+++ b/conf/airframes/esden/quady_ll11a2pwm.xml
@@ -201,7 +201,7 @@
@@ -213,12 +213,10 @@
-
-
-
+
+
+
-
-
@@ -239,7 +237,7 @@
-
diff --git a/conf/airframes/esden/quady_lm1a1pwm.xml b/conf/airframes/esden/quady_lm1a1pwm.xml
index 3ce579cf50..406734e757 100644
--- a/conf/airframes/esden/quady_lm1a1pwm.xml
+++ b/conf/airframes/esden/quady_lm1a1pwm.xml
@@ -162,7 +162,7 @@
@@ -174,12 +174,10 @@
-
-
-
+
+
+
-
-
@@ -200,7 +198,7 @@
-
diff --git a/conf/airframes/esden/quady_lm2a2pwm.xml b/conf/airframes/esden/quady_lm2a2pwm.xml
index 6be1758980..e87c817b3e 100644
--- a/conf/airframes/esden/quady_lm2a2pwm.xml
+++ b/conf/airframes/esden/quady_lm2a2pwm.xml
@@ -164,7 +164,7 @@
@@ -180,12 +180,10 @@
-
-
-
+
+
+
-
-
@@ -207,7 +205,7 @@
-
diff --git a/conf/airframes/esden/quady_lm2a2pwmppm.xml b/conf/airframes/esden/quady_lm2a2pwmppm.xml
index 298bcc8244..31fd65c39d 100644
--- a/conf/airframes/esden/quady_lm2a2pwmppm.xml
+++ b/conf/airframes/esden/quady_lm2a2pwmppm.xml
@@ -162,7 +162,7 @@
@@ -178,10 +178,8 @@
-
+
-
-
@@ -202,7 +200,7 @@
-
diff --git a/conf/airframes/esden/quady_ls01pwm.xml b/conf/airframes/esden/quady_ls01pwm.xml
new file mode 100644
index 0000000000..4a3a194c39
--- /dev/null
+++ b/conf/airframes/esden/quady_ls01pwm.xml
@@ -0,0 +1,233 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/examples/MentorEnergy.xml b/conf/airframes/examples/MentorEnergy.xml
index 9416ac28b8..80dd244402 100644
--- a/conf/airframes/examples/MentorEnergy.xml
+++ b/conf/airframes/examples/MentorEnergy.xml
@@ -21,6 +21,11 @@
+
+
+
+
+
@@ -251,4 +256,11 @@
+
+
diff --git a/conf/airframes/examples/booz2.xml b/conf/airframes/examples/booz2.xml
index 36ceee5817..e93ccf9d2c 100644
--- a/conf/airframes/examples/booz2.xml
+++ b/conf/airframes/examples/booz2.xml
@@ -194,7 +194,7 @@
@@ -210,7 +210,7 @@
-
+
diff --git a/conf/airframes/examples/h_hex.xml b/conf/airframes/examples/h_hex.xml
index ca779e54ce..5faaedebe3 100644
--- a/conf/airframes/examples/h_hex.xml
+++ b/conf/airframes/examples/h_hex.xml
@@ -189,7 +189,7 @@
-
+
diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml
index 36e979ea40..44f2681826 100644
--- a/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml
+++ b/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml
@@ -28,10 +28,11 @@
-
-
+
+
@@ -80,11 +81,6 @@
-
-
@@ -178,16 +174,16 @@
-
-
-
+
+
+
-
-
-
+
+
+
-
+
@@ -218,7 +214,7 @@
-
+
@@ -230,7 +226,7 @@
diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml
index 40786c6218..4bf55eeb9f 100644
--- a/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml
+++ b/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml
@@ -32,8 +32,9 @@
-
+
+ -->
+
@@ -83,11 +84,6 @@
-
-
@@ -181,16 +177,16 @@
-
-
-
+
+
+
-
-
-
+
+
+
-
+
@@ -221,7 +217,7 @@
-
+
@@ -233,7 +229,7 @@
diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml
index 0b31cd058e..ebdda35837 100644
--- a/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml
+++ b/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml
@@ -32,6 +32,7 @@
+
@@ -61,11 +62,6 @@
-
-
@@ -95,12 +91,12 @@
-
-
-
-
-
-
+
+
+
+
+
+
@@ -167,20 +163,20 @@
-
-
+
+
-
-
-
+
+
+
-
-
-
+
+
+
-
+
@@ -210,7 +206,7 @@
-
+
@@ -222,7 +218,7 @@
diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml b/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml
index 9f3d371545..cf85a484c0 100644
--- a/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml
+++ b/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml
@@ -29,9 +29,11 @@
-
-
+
+
@@ -56,11 +58,6 @@
-
-
@@ -162,21 +159,21 @@
-
-
+
+
-
-
-
+
+
+
-
-
-
+
+
+
-
-
-
+
+
+
@@ -205,7 +202,7 @@
-
+
@@ -217,7 +214,7 @@
diff --git a/conf/airframes/examples/lisa_asctec.xml b/conf/airframes/examples/lisa_asctec.xml
index 8b72be5c70..5e4869cb87 100644
--- a/conf/airframes/examples/lisa_asctec.xml
+++ b/conf/airframes/examples/lisa_asctec.xml
@@ -163,6 +163,7 @@
+
@@ -178,7 +179,7 @@
@@ -207,7 +208,7 @@
-
+
diff --git a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml
index e9e9aa9dba..4d68ce1534 100644
--- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml
+++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml
@@ -16,8 +16,7 @@
-
-
+
@@ -39,6 +38,8 @@
+
+
@@ -187,8 +188,8 @@
-
-
+
+
@@ -201,8 +202,10 @@
@@ -214,7 +217,7 @@
diff --git a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml
index 3511bb9772..9f7210bd3e 100644
--- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml
+++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml
@@ -16,8 +16,6 @@
-
-
@@ -40,7 +38,7 @@
-
+
@@ -206,7 +204,7 @@
diff --git a/conf/airframes/examples/quadrotor_lisa_m_mkk.xml b/conf/airframes/examples/quadrotor_lisa_m_mkk.xml
index 42fcb6b0d0..4746f8f78d 100644
--- a/conf/airframes/examples/quadrotor_lisa_m_mkk.xml
+++ b/conf/airframes/examples/quadrotor_lisa_m_mkk.xml
@@ -180,8 +180,8 @@
-
-
+
+
@@ -193,7 +193,7 @@
diff --git a/conf/airframes/examples/quadrotor_lisa_s.xml b/conf/airframes/examples/quadrotor_lisa_s.xml
new file mode 100644
index 0000000000..bc0ffa17be
--- /dev/null
+++ b/conf/airframes/examples/quadrotor_lisa_s.xml
@@ -0,0 +1,226 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/examples/quadrotor_mlkf.xml b/conf/airframes/examples/quadrotor_mlkf.xml
index c4f1066f5f..781cbd771a 100644
--- a/conf/airframes/examples/quadrotor_mlkf.xml
+++ b/conf/airframes/examples/quadrotor_mlkf.xml
@@ -6,8 +6,6 @@
-
-
@@ -192,8 +190,8 @@
-
-
+
+
@@ -206,10 +204,10 @@
diff --git a/conf/airframes/examples/quadrotor_navgo.xml b/conf/airframes/examples/quadrotor_navgo.xml
index fe108b4d6f..98df961abd 100644
--- a/conf/airframes/examples/quadrotor_navgo.xml
+++ b/conf/airframes/examples/quadrotor_navgo.xml
@@ -33,7 +33,7 @@
-
+
@@ -265,7 +265,7 @@
diff --git a/conf/airframes/examples/quadrotor_px4fmu.xml b/conf/airframes/examples/quadrotor_px4fmu.xml
new file mode 100644
index 0000000000..fb02773791
--- /dev/null
+++ b/conf/airframes/examples/quadrotor_px4fmu.xml
@@ -0,0 +1,218 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/examples/quadshot_asp21_spektrum.xml b/conf/airframes/examples/quadshot_asp21_spektrum.xml
index 1c3262b131..d5fc8439fd 100644
--- a/conf/airframes/examples/quadshot_asp21_spektrum.xml
+++ b/conf/airframes/examples/quadshot_asp21_spektrum.xml
@@ -199,7 +199,8 @@ More information on the Quadshot can be found at transition-robotics.com -->
-
+
+
@@ -231,7 +232,7 @@ More information on the Quadshot can be found at transition-robotics.com -->
diff --git a/conf/airframes/examples/stm32f4_discovery_test.xml b/conf/airframes/examples/stm32f4_discovery_test.xml
new file mode 100644
index 0000000000..ed339e5a4a
--- /dev/null
+++ b/conf/airframes/examples/stm32f4_discovery_test.xml
@@ -0,0 +1,213 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/fraser_lisa_m_rotorcraft.xml b/conf/airframes/fraser_lisa_m_rotorcraft.xml
index a232a41767..a347658ee8 100644
--- a/conf/airframes/fraser_lisa_m_rotorcraft.xml
+++ b/conf/airframes/fraser_lisa_m_rotorcraft.xml
@@ -8,14 +8,18 @@
-
+
+
+
+
+
+
-
@@ -28,12 +32,13 @@
-
+
-
+
+
@@ -64,6 +69,7 @@
+
@@ -163,8 +169,8 @@
-
-
+
+
@@ -211,19 +217,20 @@
-
-
+
+
-
+
+
-
+
@@ -231,9 +238,10 @@
diff --git a/conf/airframes/obsolete/ENAC/g1_vision.xml b/conf/airframes/obsolete/ENAC/g1_vision.xml
index 71ca37d27a..d24b588c2e 100644
--- a/conf/airframes/obsolete/ENAC/g1_vision.xml
+++ b/conf/airframes/obsolete/ENAC/g1_vision.xml
@@ -194,7 +194,7 @@
diff --git a/conf/airframes/obsolete/ENAC/mkk1.xml b/conf/airframes/obsolete/ENAC/mkk1.xml
index 9633595152..18a9742bc7 100644
--- a/conf/airframes/obsolete/ENAC/mkk1.xml
+++ b/conf/airframes/obsolete/ENAC/mkk1.xml
@@ -16,7 +16,7 @@
-
+
diff --git a/conf/airframes/obsolete/ENAC/nova1.xml b/conf/airframes/obsolete/ENAC/nova1.xml
index f3a066a809..0ef106ba39 100644
--- a/conf/airframes/obsolete/ENAC/nova1.xml
+++ b/conf/airframes/obsolete/ENAC/nova1.xml
@@ -15,7 +15,7 @@
-
+
diff --git a/conf/airframes/obsolete/Poine/booz2_a1.xml b/conf/airframes/obsolete/Poine/booz2_a1.xml
index 269bd6da8d..5823e29e03 100644
--- a/conf/airframes/obsolete/Poine/booz2_a1.xml
+++ b/conf/airframes/obsolete/Poine/booz2_a1.xml
@@ -189,7 +189,7 @@
@@ -204,7 +204,7 @@
-
+
diff --git a/conf/airframes/obsolete/Poine/booz2_a7.xml b/conf/airframes/obsolete/Poine/booz2_a7.xml
index 905a6d4d2a..d3f23c31fd 100644
--- a/conf/airframes/obsolete/Poine/booz2_a7.xml
+++ b/conf/airframes/obsolete/Poine/booz2_a7.xml
@@ -201,7 +201,7 @@
@@ -231,7 +231,7 @@
-
+
diff --git a/conf/airframes/obsolete/Poine/h_hex.xml b/conf/airframes/obsolete/Poine/h_hex.xml
index b76881c9fa..252cdacf33 100644
--- a/conf/airframes/obsolete/Poine/h_hex.xml
+++ b/conf/airframes/obsolete/Poine/h_hex.xml
@@ -168,7 +168,7 @@
-
+
diff --git a/conf/airframes/obsolete/Poine/test_libeknav.xml b/conf/airframes/obsolete/Poine/test_libeknav.xml
index 59ec1b8ac7..344e8db8b3 100644
--- a/conf/airframes/obsolete/Poine/test_libeknav.xml
+++ b/conf/airframes/obsolete/Poine/test_libeknav.xml
@@ -1,4 +1,4 @@
-
+
diff --git a/conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml b/conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml
index a79cea2d5c..10b61e41a0 100644
--- a/conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml
+++ b/conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml
@@ -263,7 +263,7 @@ second attempt
@@ -291,7 +291,7 @@ second attempt
-
+
diff --git a/conf/airframes/obsolete/UofAdelaide/A1000_LISA.xml b/conf/airframes/obsolete/UofAdelaide/A1000_LISA.xml
index ebd6bd5bbe..5f3ee2a945 100644
--- a/conf/airframes/obsolete/UofAdelaide/A1000_LISA.xml
+++ b/conf/airframes/obsolete/UofAdelaide/A1000_LISA.xml
@@ -200,7 +200,7 @@
-
+
diff --git a/conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml b/conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml
index 5d38914b96..406d6de2c0 100644
--- a/conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml
+++ b/conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml
@@ -216,7 +216,7 @@
@@ -249,7 +249,7 @@
-
+
diff --git a/conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml b/conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml
index 06ea8ce8ad..82f499852b 100644
--- a/conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml
+++ b/conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml
@@ -265,7 +265,7 @@ second attempt
diff --git a/conf/airframes/obsolete/booz2_a1p.xml b/conf/airframes/obsolete/booz2_a1p.xml
index f5906c89d6..18ff3d98f8 100644
--- a/conf/airframes/obsolete/booz2_a1p.xml
+++ b/conf/airframes/obsolete/booz2_a1p.xml
@@ -193,7 +193,7 @@
diff --git a/conf/airframes/obsolete/booz2_s1.xml b/conf/airframes/obsolete/booz2_s1.xml
index 17b8eaf5b5..4cf6bb4bbe 100644
--- a/conf/airframes/obsolete/booz2_s1.xml
+++ b/conf/airframes/obsolete/booz2_s1.xml
@@ -180,7 +180,7 @@
diff --git a/conf/airframes/obsolete/example_heli_lisam.xml b/conf/airframes/obsolete/example_heli_lisam.xml
index 6ed0bf3d66..79e8d519ac 100644
--- a/conf/airframes/obsolete/example_heli_lisam.xml
+++ b/conf/airframes/obsolete/example_heli_lisam.xml
@@ -217,7 +217,7 @@ AP_MODE_NAV
@@ -252,7 +252,7 @@ AP_MODE_NAV
-
+
diff --git a/conf/airframes/obsolete/mm/rotor/qmk1.xml b/conf/airframes/obsolete/mm/rotor/qmk1.xml
index 7d561384d0..37b3b6c07a 100644
--- a/conf/airframes/obsolete/mm/rotor/qmk1.xml
+++ b/conf/airframes/obsolete/mm/rotor/qmk1.xml
@@ -174,7 +174,7 @@
@@ -200,7 +200,7 @@
-
+
diff --git a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_l_mkk.xml b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_l_mkk.xml
index 93991b0de9..5d1b3e397d 100644
--- a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_l_mkk.xml
+++ b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_l_mkk.xml
@@ -218,7 +218,7 @@
diff --git a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_1_pwm.xml b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_1_pwm.xml
index 7fabac6e96..5343031357 100644
--- a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_1_pwm.xml
+++ b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_1_pwm.xml
@@ -180,7 +180,7 @@
diff --git a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_2_pwm.xml b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_2_pwm.xml
index ed99bdefa5..2ba487a784 100644
--- a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_2_pwm.xml
+++ b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_2_pwm.xml
@@ -187,7 +187,7 @@
diff --git a/conf/boards/ardrone2_raw.makefile b/conf/boards/ardrone2_raw.makefile
index 5ab2d5f7cc..b97997b11b 100644
--- a/conf/boards/ardrone2_raw.makefile
+++ b/conf/boards/ardrone2_raw.makefile
@@ -15,11 +15,17 @@ $(TARGET).ARCHDIR = $(ARCH)
# -----------------------------------------------------------------------
USER=foobar
-HOST=192.168.1.1
+HOST?=192.168.1.1
SUB_DIR=raw
FTP_DIR=/data/video
TARGET_DIR=$(FTP_DIR)/$(SUB_DIR)
# -----------------------------------------------------------------------
+ARDRONE2_START_PAPARAZZI ?= 0
+ARDRONE2_WIFI_MODE ?= 0
+ARDRONE2_SSID ?= ardrone2_paparazzi
+ARDRONE2_IP_ADDRESS_BASE ?= 192.168.1.
+ARDRONE2_IP_ADDRESS_PROBE ?= 1
+# -----------------------------------------------------------------------
# The GPS sensor is connected trough USB so we have to define the device
GPS_PORT ?= UART1
@@ -35,8 +41,8 @@ $(TARGET).CFLAGS +=-DARDRONE2_RAW
# -----------------------------------------------------------------------
# default LED configuration
-RADIO_CONTROL_LED ?= none
-BARO_LED ?= none
-AHRS_ALIGNER_LED ?= none
-GPS_LED ?= none
-SYS_TIME_LED ?= none
+RADIO_CONTROL_LED ?= 6
+BARO_LED ?= none
+AHRS_ALIGNER_LED ?= 5
+GPS_LED ?= 3
+SYS_TIME_LED ?= 0
diff --git a/conf/boards/ardrone2_sdk.makefile b/conf/boards/ardrone2_sdk.makefile
index 46ba01c8c9..6ee6311af1 100644
--- a/conf/boards/ardrone2_sdk.makefile
+++ b/conf/boards/ardrone2_sdk.makefile
@@ -20,6 +20,12 @@ SUB_DIR=sdk
FTP_DIR=/data/video
TARGET_DIR=$(FTP_DIR)/$(SUB_DIR)
# -----------------------------------------------------------------------
+ARDRONE2_START_PAPARAZZI ?= 0
+ARDRONE2_WIFI_MODE ?= 0
+ARDRONE2_SSID ?= ardrone2_paparazzi
+ARDRONE2_IP_ADDRESS_BASE ?= 192.168.1.
+ARDRONE2_IP_ADDRESS_PROBE ?= 1
+# -----------------------------------------------------------------------
# The GPS sensor is connected trough USB so we have to define the device
GPS_PORT ?= UART1
@@ -32,3 +38,10 @@ $(TARGET).CFLAGS += -DUART1_DEV=\"/dev/ttyUSB0\"
ap.CFLAGS +=-DARDRONE2_SDK
# -----------------------------------------------------------------------
+
+# default LED configuration
+RADIO_CONTROL_LED ?= none
+BARO_LED ?= none
+AHRS_ALIGNER_LED ?= none
+GPS_LED ?= none
+SYS_TIME_LED ?= none
diff --git a/conf/boards/lisa_s_0.1.makefile b/conf/boards/lisa_s_0.1.makefile
new file mode 100644
index 0000000000..51d99547e3
--- /dev/null
+++ b/conf/boards/lisa_s_0.1.makefile
@@ -0,0 +1,72 @@
+# Hey Emacs, this is a -*- makefile -*-
+#
+# lisa_s_0.1.makefile
+#
+# http://paparazzi.enac.fr/wiki/Lisa/S
+#
+
+BOARD=lisa_s
+BOARD_VERSION=0.1
+BOARD_CFG=\"boards/$(BOARD)_$(BOARD_VERSION).h\"
+
+ARCH=stm32
+$(TARGET).ARCHDIR = $(ARCH)
+$(TARGET).LDSCRIPT=$(SRC_ARCH)/lisa-s.ld
+
+# -----------------------------------------------------------------------
+
+# default flash mode is via usb dfu bootloader (luftboot)
+# other possibilities: JTAG, SWD, SERIAL
+FLASH_MODE ?= SWD
+
+#
+#
+# some default values shared between different firmwares
+#
+#
+
+#
+# default LED configuration
+#
+RADIO_CONTROL_LED ?= 3
+BARO_LED ?= none
+AHRS_ALIGNER_LED ?= 2
+GPS_LED ?= none
+SYS_TIME_LED ?= 1
+
+#
+# default uart configuration
+#
+RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT ?= UART2
+RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT ?= none
+
+MODEM_PORT ?= UART1
+MODEM_BAUD ?= B57600
+
+GPS_PORT ?= UART3
+GPS_BAUD ?= B38400
+
+#
+# default actuator configuration
+#
+# you can use different actuators by adding a configure option to your firmware section
+# e.g.
+#
+ACTUATORS ?= actuators_pwm
+
+# Thish should be disabled as we don't have adc inputs on Lisa/S
+ifndef ADC_IR1
+ADC_IR1 = 1
+ADC_IR1_CHAN = 0
+endif
+ifndef ADC_IR2
+ADC_IR2 = 2
+ADC_IR2_CHAN = 1
+endif
+ifndef ADC_IR3
+ADC_IR_TOP = 3
+ADC_IR_TOP_CHAN = 2
+endif
+ADC_IR_NB_SAMPLES ?= 16
diff --git a/conf/boards/px4fmu_1.7.makefile b/conf/boards/px4fmu_1.7.makefile
new file mode 100644
index 0000000000..798d46d7a5
--- /dev/null
+++ b/conf/boards/px4fmu_1.7.makefile
@@ -0,0 +1,55 @@
+# Hey Emacs, this is a -*- makefile -*-
+#
+# px4fmu_1.7.makefile
+#
+#
+
+BOARD=px4fmu
+BOARD_VERSION=1.7
+BOARD_CFG=\"boards/$(BOARD)_$(BOARD_VERSION).h\"
+
+ARCH=stm32
+ARCH_L=f4
+ARCH_DIR=stm32
+SRC_ARCH=arch/$(ARCH_DIR)
+$(TARGET).ARCHDIR = $(ARCH)
+$(TARGET).LDSCRIPT=$(SRC_ARCH)/apogee.ld
+
+HARD_FLOAT=yes
+
+# default flash mode is via usb dfu bootloader
+# possibilities: DFU, SWD
+FLASH_MODE ?= SWD
+
+
+#
+# default LED configuration
+#
+RADIO_CONTROL_LED ?= none
+BARO_LED ?= none
+AHRS_ALIGNER_LED ?= 2
+GPS_LED ?= none
+SYS_TIME_LED ?= 1
+
+#
+# default UART configuration (modem, gps, spektrum)
+#
+
+MODEM_PORT ?= UART1
+MODEM_BAUD ?= B57600
+
+GPS_PORT ?= UART6
+GPS_BAUD ?= B38400
+
+RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT ?= UART2
+
+
+#
+# default actuator configuration
+#
+# you can use different actuators by adding a configure option to your firmware section
+# e.g.
+#
+ACTUATORS ?= actuators_pwm
diff --git a/conf/boards/stm32f4_discovery.makefile b/conf/boards/stm32f4_discovery.makefile
new file mode 100644
index 0000000000..f00700c783
--- /dev/null
+++ b/conf/boards/stm32f4_discovery.makefile
@@ -0,0 +1,57 @@
+# Hey Emacs, this is a -*- makefile -*-
+#
+# stm32f4_discovery.makefile
+#
+#
+
+BOARD=stm32f4_discovery
+BOARD_VERSION=
+BOARD_CFG=\"boards/$(BOARD).h\"
+
+ARCH=stm32
+ARCH_L=f4
+ARCH_DIR=stm32
+SRC_ARCH=arch/$(ARCH_DIR)
+$(TARGET).ARCHDIR = $(ARCH)
+$(TARGET).LDSCRIPT=$(SRC_ARCH)/apogee.ld
+
+HARD_FLOAT=yes
+
+# default flash mode is via usb dfu bootloader
+# possibilities: DFU, SWD
+FLASH_MODE ?= DFU
+STLINK ?= n
+DFU_UTIL ?= y
+
+#
+# default LED configuration
+#
+RADIO_CONTROL_LED ?= 4
+BARO_LED ?= none
+AHRS_ALIGNER_LED ?= none
+GPS_LED ?= none
+SYS_TIME_LED ?= 3
+
+#
+# default UART configuration (modem, gps, spektrum)
+#
+
+MODEM_PORT ?= UART6
+MODEM_BAUD ?= B57600
+
+GPS_PORT ?= UART4
+GPS_BAUD ?= B38400
+
+RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT ?= UART2
+
+
+#
+# default actuator configuration
+#
+# you can use different actuators by adding a configure option to your firmware section
+# e.g.
+#
+ACTUATORS ?= actuators_pwm
+
diff --git a/conf/conf.xml.example b/conf/conf_example.xml
similarity index 93%
rename from conf/conf.xml.example
rename to conf/conf_example.xml
index eac6d746c4..893aba9cec 100644
--- a/conf/conf.xml.example
+++ b/conf/conf_example.xml
@@ -44,6 +44,16 @@
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/control/stabilization_rate.xml"
gui_color="white"
/>
+
with in your airframe file.)
+$(error The stabilization euler subsystem has been renamed, please replace with in your airframe file.)
-include $(PAPARAZZI_SRC)/conf/firmwares/subsystems/rotorcraft/stabilization_int_euler.makefile
diff --git a/conf/firmwares/subsystems/rotorcraft/telemetry_superbitrf.makefile b/conf/firmwares/subsystems/rotorcraft/telemetry_superbitrf.makefile
new file mode 100644
index 0000000000..7c784cdcd2
--- /dev/null
+++ b/conf/firmwares/subsystems/rotorcraft/telemetry_superbitrf.makefile
@@ -0,0 +1,14 @@
+#
+# The superbitRF module as telemetry downlink/uplink
+#
+#
+ap.CFLAGS += -DUSE_$(MODEM_PORT)
+ap.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
+
+ap.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=SuperbitRF
+ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=SUPERBITRF
+#ap.CFLAGS += -DUSE_SUPERBITRF -DUSE_SPI2 -DUSE_SPI_SLAVE2
+
+ap.srcs += peripherals/cyrf6936.c
+ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/superbitrf.c subsystems/datalink/pprz_transport.c
+ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/telemetry.c
diff --git a/conf/firmwares/subsystems/shared/actuators_asctec.makefile b/conf/firmwares/subsystems/shared/actuators_asctec.makefile
index be5d0707ff..5551d61262 100644
--- a/conf/firmwares/subsystems/shared/actuators_asctec.makefile
+++ b/conf/firmwares/subsystems/shared/actuators_asctec.makefile
@@ -7,17 +7,38 @@
#
$(TARGET).CFLAGS += -DACTUATORS
-ap.srcs += subsystems/actuators/actuators_asctec.c
+ACTUATORS_ASCTEC_SRCS = subsystems/actuators/actuators_asctec.c
+
+
+# set default i2c device if not already configured
+ifeq ($(ARCH), lpc21)
+ACTUATORS_ASCTEC_I2C_DEV ?= i2c0
+else ifeq ($(ARCH), stm32)
+ACTUATORS_ASCTEC_I2C_DEV ?= i2c1
+endif
+
+ifeq ($(TARGET), ap)
+ifndef ACTUATORS_ASCTEC_I2C_DEV
+$(error Error: ACTUATORS_ASCTEC_I2C_DEV not configured!)
+endif
+endif
+
+# convert i2cx to upper/lower case
+ACTUATORS_ASCTEC_I2C_DEV_UPPER=$(shell echo $(ACTUATORS_ASCTEC_I2C_DEV) | tr a-z A-Z)
+ACTUATORS_ASCTEC_I2C_DEV_LOWER=$(shell echo $(ACTUATORS_ASCTEC_I2C_DEV) | tr A-Z a-z)
+
+ACTUATORS_ASCTEC_CFLAGS += -DACTUATORS_ASCTEC_I2C_DEV=$(ACTUATORS_ASCTEC_I2C_DEV_LOWER)
+ACTUATORS_ASCTEC_CFLAGS += -DUSE_$(ACTUATORS_ASCTEC_I2C_DEV_UPPER)
ifeq ($(ARCH), lpc21)
-ap.CFLAGS += -DACTUATORS_ASCTEC_I2C_DEV=i2c0
-ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150
+# set default i2c timing if not already configured
+ACTUATORS_ASCTEC_I2C_SCL_TIME ?= 150
+ACTUATORS_ASCTEC_CFLAGS += -D$(ACTUATORS_ASCTEC_I2C_DEV_UPPER)_SCLL=$(ACTUATORS_ASCTEC_I2C_SCL_TIME)
+ACTUATORS_ASCTEC_CFLAGS += -D$(ACTUATORS_ASCTEC_I2C_DEV_UPPER)_SCLH=$(ACTUATORS_ASCTEC_I2C_SCL_TIME)
endif
-ifeq ($(ARCH), stm32)
-ap.CFLAGS += -DACTUATORS_ASCTEC_I2C_DEV=i2c1
-ap.CFLAGS += -DUSE_I2C1
-endif
+ap.CFLAGS += $(ACTUATORS_ASCTEC_CFLAGS)
+ap.srcs += $(ACTUATORS_ASCTEC_SRCS)
# Simulator
diff --git a/conf/firmwares/subsystems/shared/actuators_asctec_v2.makefile b/conf/firmwares/subsystems/shared/actuators_asctec_v2.makefile
index c2f8807796..94bdf097cd 100644
--- a/conf/firmwares/subsystems/shared/actuators_asctec_v2.makefile
+++ b/conf/firmwares/subsystems/shared/actuators_asctec_v2.makefile
@@ -7,17 +7,39 @@
#
$(TARGET).CFLAGS += -DACTUATORS
-ap.srcs += subsystems/actuators/actuators_asctec_v2.c
+ACTUATORS_ASCTEC_SRCS = subsystems/actuators/actuators_asctec_v2.c
+
+
+# set default i2c device if not already configured
+ifeq ($(ARCH), lpc21)
+ACTUATORS_ASCTEC_V2_I2C_DEV ?= i2c0
+else ifeq ($(ARCH), stm32)
+ACTUATORS_ASCTEC_V2_I2C_DEV ?= i2c1
+endif
+
+ifeq ($(TARGET), ap)
+ifndef ACTUATORS_ASCTEC_V2_I2C_DEV
+$(error Error: ACTUATORS_ASCTEC_V2_I2C_DEV not configured!)
+endif
+endif
+
+# convert i2cx to upper/lower case
+ACTUATORS_ASCTEC_V2_I2C_DEV_UPPER=$(shell echo $(ACTUATORS_ASCTEC_V2_I2C_DEV) | tr a-z A-Z)
+ACTUATORS_ASCTEC_V2_I2C_DEV_LOWER=$(shell echo $(ACTUATORS_ASCTEC_V2_I2C_DEV) | tr A-Z a-z)
+
+ACTUATORS_ASCTEC_V2_CFLAGS += -DACTUATORS_ASCTEC_V2_I2C_DEV=$(ACTUATORS_ASCTEC_V2_I2C_DEV_LOWER)
+ACTUATORS_ASCTEC_V2_CFLAGS += -DUSE_$(ACTUATORS_ASCTEC_V2_I2C_DEV_UPPER)
ifeq ($(ARCH), lpc21)
-ap.CFLAGS += -DACTUATORS_ASCTEC_V2_I2C_DEV=i2c0
-ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150
+# set default i2c timing if not already configured
+ACTUATORS_ASCTEC_V2_I2C_SCL_TIME ?= 150
+ACTUATORS_ASCTEC_V2_CFLAGS += -D$(ACTUATORS_ASCTEC_V2_I2C_DEV_UPPER)_SCLL=$(ACTUATORS_ASCTEC_V2_I2C_SCL_TIME)
+ACTUATORS_ASCTEC_V2_CFLAGS += -D$(ACTUATORS_ASCTEC_V2_I2C_DEV_UPPER)_SCLH=$(ACTUATORS_ASCTEC_V2_I2C_SCL_TIME)
endif
-ifeq ($(ARCH), stm32)
-ap.CFLAGS += -DACTUATORS_ASCTEC_V2_I2C_DEV=i2c1
-ap.CFLAGS += -DUSE_I2C1
-endif
+ap.CFLAGS += $(ACTUATORS_ASCTEC_V2_CFLAGS)
+ap.srcs += $(ACTUATORS_ASCTEC_V2_SRCS)
+
# Simulator
diff --git a/conf/firmwares/subsystems/shared/actuators_mkk.makefile b/conf/firmwares/subsystems/shared/actuators_mkk.makefile
index e602f595fa..fa235d2a82 100644
--- a/conf/firmwares/subsystems/shared/actuators_mkk.makefile
+++ b/conf/firmwares/subsystems/shared/actuators_mkk.makefile
@@ -22,23 +22,39 @@
# max command = 255
$(TARGET).CFLAGS += -DACTUATORS
-ap.srcs += subsystems/actuators/actuators_mkk.c
+
+ACTUATORS_MKK_SRCS = subsystems/actuators/actuators_mkk.c
+
ifeq ($(ARCH), lpc21)
-
-# set default i2c timing if not already configured
-ifeq ($(MKK_I2C_SCL_TIME), )
-MKK_I2C_SCL_TIME=150
-endif
-
-ap.CFLAGS += -DACTUATORS_MKK_I2C_DEV=i2c0
-ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=$(MKK_I2C_SCL_TIME) -DI2C0_SCLH=$(MKK_I2C_SCL_TIME)
-
+ACTUATORS_MKK_I2C_DEV ?= i2c0
else ifeq ($(ARCH), stm32)
-ap.CFLAGS += -DACTUATORS_MKK_I2C_DEV=i2c1
-ap.CFLAGS += -DUSE_I2C1
+ACTUATORS_MKK_I2C_DEV ?= i2c1
endif
+ifeq ($(TARGET), ap)
+ifndef ACTUATORS_MKK_I2C_DEV
+$(error Error: ACTUATORS_MKK_I2C_DEV not configured!)
+endif
+endif
+
+# convert i2cx to upper/lower case
+ACTUATORS_MKK_I2C_DEV_UPPER=$(shell echo $(ACTUATORS_MKK_I2C_DEV) | tr a-z A-Z)
+ACTUATORS_MKK_I2C_DEV_LOWER=$(shell echo $(ACTUATORS_MKK_I2C_DEV) | tr A-Z a-z)
+
+ACTUATORS_MKK_CFLAGS += -DACTUATORS_MKK_I2C_DEV=$(ACTUATORS_MKK_I2C_DEV_LOWER)
+ACTUATORS_MKK_CFLAGS += -DUSE_$(ACTUATORS_MKK_I2C_DEV_UPPER)
+
+ifeq ($(ARCH), lpc21)
+# set default i2c timing if not already configured
+ACTUATORS_MKK_I2C_SCL_TIME ?= 150
+ACTUATORS_MKK_CFLAGS += -D$(ACTUATORS_MKK_I2C_DEV_UPPER)_SCLL=$(ACTUATORS_MKK_I2C_SCL_TIME)
+ACTUATORS_MKK_CFLAGS += -D$(ACTUATORS_MKK_I2C_DEV_UPPER)_SCLH=$(ACTUATORS_MKK_I2C_SCL_TIME)
+endif
+
+ap.CFLAGS += $(ACTUATORS_MKK_CFLAGS)
+ap.srcs += $(ACTUATORS_MKK_SRCS)
+
# Simulator
nps.srcs += subsystems/actuators/actuators_mkk.c
nps.CFLAGS += -DUSE_I2C0 -DACTUATORS_MKK_I2C_DEV=i2c0
diff --git a/conf/firmwares/subsystems/shared/actuators_mkk_v2.makefile b/conf/firmwares/subsystems/shared/actuators_mkk_v2.makefile
index 4246e1b90a..1b539a61ea 100644
--- a/conf/firmwares/subsystems/shared/actuators_mkk_v2.makefile
+++ b/conf/firmwares/subsystems/shared/actuators_mkk_v2.makefile
@@ -22,23 +22,38 @@
# max command = 2047
$(TARGET).CFLAGS += -DACTUATORS
-ap.srcs += subsystems/actuators/actuators_mkk_v2.c
+ACTUATORS_MKK_V2_SRCS = subsystems/actuators/actuators_mkk_v2.c
+
ifeq ($(ARCH), lpc21)
-
-# set default i2c timing if not already configured
-ifeq ($(MKK_V2_I2C_SCL_TIME), )
-MKK_V2_I2C2_SCL_TIME=150
-endif
-
-ap.CFLAGS += -DACTUATORS_MKK_V2_I2C_DEV=i2c0
-ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=$(MKK_V2_I2C_SCL_TIME) -DI2C0_SCLH=$(MKK_V2_I2C_SCL_TIME)
-
+ACTUATORS_MKK_V2_I2C_DEV ?= i2c0
else ifeq ($(ARCH), stm32)
-ap.CFLAGS += -DACTUATORS_MKK_V2_I2C_DEV=i2c1
-ap.CFLAGS += -DUSE_I2C1
+ACTUATORS_MKK_V2_I2C_DEV ?= i2c1
endif
+ifeq ($(TARGET), ap)
+ifndef ACTUATORS_MKK_V2_I2C_DEV
+$(error Error: ACTUATORS_MKK_V2_I2C_DEV not configured!)
+endif
+endif
+
+# convert i2cx to upper/lower case
+ACTUATORS_MKK_V2_I2C_DEV_UPPER=$(shell echo $(ACTUATORS_MKK_V2_I2C_DEV) | tr a-z A-Z)
+ACTUATORS_MKK_V2_I2C_DEV_LOWER=$(shell echo $(ACTUATORS_MKK_V2_I2C_DEV) | tr A-Z a-z)
+
+ACTUATORS_MKK_V2_CFLAGS += -DACTUATORS_MKK_V2_I2C_DEV=$(ACTUATORS_MKK_V2_I2C_DEV_LOWER)
+ACTUATORS_MKK_V2_CFLAGS += -DUSE_$(ACTUATORS_MKK_V2_I2C_DEV_UPPER)
+
+ifeq ($(ARCH), lpc21)
+# set default i2c timing if not already configured
+ACTUATORS_MKK_V2_I2C_SCL_TIME ?= 150
+ACTUATORS_MKK_V2_CFLAGS += -D$(ACTUATORS_MKK_V2_I2C_DEV_UPPER)_SCLL=$(ACTUATORS_MKK_V2_I2C_SCL_TIME)
+ACTUATORS_MKK_V2_CFLAGS += -D$(ACTUATORS_MKK_V2_I2C_DEV_UPPER)_SCLH=$(ACTUATORS_MKK_V2_I2C_SCL_TIME)
+endif
+
+ap.CFLAGS += $(ACTUATORS_MKK_V2_CFLAGS)
+ap.srcs += $(ACTUATORS_MKK_V2_SRCS)
+
# Simulator:
nps.srcs += subsystems/actuators/actuators_mkk_v2.c
nps.CFLAGS += -DUSE_I2C0 -DACTUATORS_MKK_V2_I2C_DEV=i2c0
diff --git a/conf/firmwares/subsystems/shared/actuators_skiron.makefile b/conf/firmwares/subsystems/shared/actuators_skiron.makefile
index 5296056e43..00ff9d3574 100644
--- a/conf/firmwares/subsystems/shared/actuators_skiron.makefile
+++ b/conf/firmwares/subsystems/shared/actuators_skiron.makefile
@@ -17,20 +17,41 @@
# command_laws section to map motor_mixing commands to servos
#
-# set default i2c timing if not already configured
-ifeq ($(SKIRON_I2C_SCL_TIME), )
-SKIRON_I2C_SCL_TIME=150
+$(TARGET).CFLAGS += -DACTUATORS
+ACTUATORS_SKIRON_SRCS = subsystems/actuators/actuators_skiron.c
+
+# set default i2c device if not already configured
+ifeq ($(ARCH), lpc21)
+ACTUATORS_SKIRON_I2C_DEV ?= i2c0
+else ifeq ($(ARCH), stm32)
+ACTUATORS_SKIRON_I2C_DEV ?= i2c1
endif
-$(TARGET).CFLAGS += -DACTUATORS
-ap.srcs += subsystems/actuators/actuators_skiron.c
+ifeq ($(TARGET), ap)
+ifndef ACTUATORS_SKIRON_I2C_DEV
+$(error Error: ACTUATORS_SKIRON_I2C_DEV not configured!)
+endif
+endif
+
+# convert i2cx to upper/lower case
+ACTUATORS_SKIRON_I2C_DEV_UPPER=$(shell echo $(ACTUATORS_SKIRON_I2C_DEV) | tr a-z A-Z)
+ACTUATORS_SKIRON_I2C_DEV_LOWER=$(shell echo $(ACTUATORS_SKIRON_I2C_DEV) | tr A-Z a-z)
+
+ACTUATORS_SKIRON_CFLAGS += -DACTUATORS_SKIRON_I2C_DEV=$(ACTUATORS_SKIRON_I2C_DEV_LOWER)
+ACTUATORS_SKIRON_CFLAGS += -DUSE_$(ACTUATORS_SKIRON_I2C_DEV_UPPER)
ifeq ($(ARCH), lpc21)
-ap.CFLAGS += -DACTUATORS_SKIRON_I2C_DEV=i2c0
-ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=$(SKIRON_I2C_SCL_TIME) -DI2C0_SCLH=$(SKIRON_I2C_SCL_TIME)
+# set default i2c timing if not already configured
+ACTUATORS_SKIRON_I2C_SCL_TIME ?= 150
+ACTUATORS_SKIRON_CFLAGS += -D$(ACTUATORS_SKIRON_I2C_DEV_UPPER)_SCLL=$(ACTUATORS_SKIRON_I2C_SCL_TIME)
+ACTUATORS_SKIRON_CFLAGS += -D$(ACTUATORS_SKIRON_I2C_DEV_UPPER)_SCLH=$(ACTUATORS_SKIRON_I2C_SCL_TIME)
endif
+ap.CFLAGS += $(ACTUATORS_SKIRON_CFLAGS)
+ap.srcs += $(ACTUATORS_SKIRON_SRCS)
+
+
# Simulator
nps.srcs += subsystems/actuators/actuators_skiron.c
-nps.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=$(SKIRON_I2C_SCL_TIME) -DI2C0_SCLH=$(SKIRON_I2C_SCL_TIME) -DACTUATORS_SKIRON_I2C_DEV=i2c0
+nps.CFLAGS += -DUSE_I2C0 -DACTUATORS_SKIRON_I2C_DEV=i2c0
diff --git a/conf/firmwares/subsystems/shared/imu_aspirin2_i2c.makefile b/conf/firmwares/subsystems/shared/imu_aspirin2_i2c.makefile
index e2cc60d604..968a6c9823 100644
--- a/conf/firmwares/subsystems/shared/imu_aspirin2_i2c.makefile
+++ b/conf/firmwares/subsystems/shared/imu_aspirin2_i2c.makefile
@@ -7,14 +7,31 @@ IMU_ASPIRIN2_SRCS = $(SRC_SUBSYSTEMS)/imu.c \
$(SRC_MODULES)/sensors/imu_aspirin2.c
-IMU_ASPIRIN2_CFLAGS += -DUSE_I2C
-ifeq ($(ARCH), stm32)
- IMU_ASPIRIN2_CFLAGS += -DUSE_I2C2
- IMU_ASPIRIN2_CFLAGS += -DPPZUAVIMU_I2C_DEV=i2c2
-else ifeq ($(ARCH), lpc21)
- IMU_ASPIRIN2_CFLAGS += -DUSE_I2C0
- IMU_ASPIRIN2_CFLAGS += -DPPZUAVIMU_I2C_DEV=i2c0
+# set default i2c bus
+ifeq ($(ARCH), lpc21)
+IMU_ASPIRIN2_I2C_DEV ?= i2c0
+else ifeq ($(ARCH), stm32)
+IMU_ASPIRIN2_I2C_DEV ?= i2c2
endif
+ifeq ($(TARGET), ap)
+ifndef IMU_ASPIRIN2_I2C_DEV
+$(error Error: IMU_ASPIRIN2_I2C_DEV not configured!)
+endif
+endif
+
+# convert i2cx to upper/lower case
+IMU_ASPIRIN2_I2C_DEV_UPPER=$(shell echo $(IMU_ASPIRIN2_I2C_DEV) | tr a-z A-Z)
+IMU_ASPIRIN2_I2C_DEV_LOWER=$(shell echo $(IMU_ASPIRIN2_I2C_DEV) | tr A-Z a-z)
+
+IMU_ASPIRIN2_CFLAGS += -DIMU_ASPIRIN2_I2C_DEV=$(IMU_ASPIRIN2_I2C_DEV_LOWER)
+IMU_ASPIRIN2_CFLAGS += -DUSE_$(IMU_ASPIRIN2_I2C_DEV_UPPER)
+
ap.CFLAGS += $(IMU_ASPIRIN2_CFLAGS)
ap.srcs += $(IMU_ASPIRIN2_SRCS)
+
+
+#
+# NPS simulator
+#
+include $(CFG_SHARED)/imu_nps.makefile
diff --git a/conf/firmwares/subsystems/shared/imu_aspirin_common.makefile b/conf/firmwares/subsystems/shared/imu_aspirin_common.makefile
index a19bb4a2b8..673096f75c 100644
--- a/conf/firmwares/subsystems/shared/imu_aspirin_common.makefile
+++ b/conf/firmwares/subsystems/shared/imu_aspirin_common.makefile
@@ -44,29 +44,57 @@ IMU_ASPIRIN_SRCS += peripherals/itg3200.c
#IMU_ASPIRIN_SRCS += peripherals/hmc5843.c $(SRC_ARCH)/peripherals/hmc5843_arch.c
IMU_ASPIRIN_SRCS += peripherals/hmc58xx.c
+
+# set default i2c bus
ifeq ($(ARCH), lpc21)
-IMU_ASPIRIN_CFLAGS += -DUSE_SPI_SLAVE0
-IMU_ASPIRIN_CFLAGS += -DASPIRIN_SPI_SLAVE_IDX=SPI_SLAVE0
-IMU_ASPIRIN_CFLAGS += -DASPIRIN_SPI_DEV=spi1
-IMU_ASPIRIN_CFLAGS += -DUSE_SPI1
-ifndef ASPIRIN_I2C_DEV
-ASPIRIN_I2C_DEV=i2c0
-endif
+ASPIRIN_I2C_DEV ?= i2c0
else ifeq ($(ARCH), stm32)
-IMU_ASPIRIN_CFLAGS += -DUSE_SPI2
+ASPIRIN_I2C_DEV ?= i2c2
+endif
+
+ifeq ($(TARGET), ap)
+ifndef ASPIRIN_I2C_DEV
+$(error Error: ASPIRIN_I2C_DEV not configured!)
+endif
+endif
+
+# convert i2cx to upper/lower case
+ASPIRIN_I2C_DEV_UPPER=$(shell echo $(ASPIRIN_I2C_DEV) | tr a-z A-Z)
+ASPIRIN_I2C_DEV_LOWER=$(shell echo $(ASPIRIN_I2C_DEV) | tr A-Z a-z)
+
+IMU_ASPIRIN_CFLAGS += -DASPIRIN_I2C_DEV=$(ASPIRIN_I2C_DEV_LOWER)
+IMU_ASPIRIN_CFLAGS += -DUSE_$(ASPIRIN_I2C_DEV_UPPER)
+
+
+# set default SPI device and slave index
+ifeq ($(ARCH), lpc21)
+ASPIRIN_SPI_DEV ?= spi1
+ASPIRIN_SPI_SLAVE_IDX ?= SPI_SLAVE0
+else ifeq ($(ARCH), stm32)
+ASPIRIN_SPI_DEV ?= spi2
# Slave select configuration
# SLAVE2 is on PB12 (NSS) (ADXL345 CS)
-IMU_ASPIRIN_CFLAGS += -DUSE_SPI_SLAVE2
-ifndef ASPIRIN_I2C_DEV
-ASPIRIN_I2C_DEV=i2c2
+ASPIRIN_SPI_SLAVE_IDX ?= SPI_SLAVE2
+endif
+
+ifeq ($(TARGET), ap)
+ifndef ASPIRIN_SPI_DEV
+$(error Error: ASPIRIN_SPI_DEV not configured!)
+endif
+ifndef ASPIRIN_SPI_SLAVE_IDX
+$(error Error: ASPIRIN_SPI_SLAVE_IDX not configured!)
endif
endif
-# convert i2cx to upper case
-ASPIRIN_I2C_DEV_UPPER=$(shell echo $(ASPIRIN_I2C_DEV) | tr a-z A-Z)
+# convert spix to upper/lower case
+ASPIRIN_SPI_DEV_UPPER=$(shell echo $(ASPIRIN_SPI_DEV) | tr a-z A-Z)
+ASPIRIN_SPI_DEV_LOWER=$(shell echo $(ASPIRIN_SPI_DEV) | tr A-Z a-z)
+
+IMU_ASPIRIN_CFLAGS += -DASPIRIN_SPI_DEV=$(ASPIRIN_SPI_DEV_LOWER)
+IMU_ASPIRIN_CFLAGS += -DUSE_$(ASPIRIN_SPI_DEV_UPPER)
+IMU_ASPIRIN_CFLAGS += -DASPIRIN_SPI_SLAVE_IDX=$(ASPIRIN_SPI_SLAVE_IDX)
+IMU_ASPIRIN_CFLAGS += -DUSE_$(ASPIRIN_SPI_SLAVE_IDX)
-IMU_ASPIRIN_CFLAGS += -DASPIRIN_I2C_DEV=$(ASPIRIN_I2C_DEV)
-IMU_ASPIRIN_CFLAGS += -DUSE_$(ASPIRIN_I2C_DEV_UPPER)
#
# NPS simulator
diff --git a/conf/firmwares/subsystems/shared/imu_aspirin_i2c_common.makefile b/conf/firmwares/subsystems/shared/imu_aspirin_i2c_common.makefile
index 064ceb3d03..b5b4618a57 100644
--- a/conf/firmwares/subsystems/shared/imu_aspirin_i2c_common.makefile
+++ b/conf/firmwares/subsystems/shared/imu_aspirin_i2c_common.makefile
@@ -38,19 +38,26 @@ IMU_ASPIRIN_SRCS += peripherals/itg3200.c
# Magnetometer
IMU_ASPIRIN_SRCS += peripherals/hmc58xx.c
+
# set default i2c bus
-ifndef ASPIRIN_I2C_DEV
ifeq ($(ARCH), lpc21)
-ASPIRIN_I2C_DEV=i2c0
+ASPIRIN_I2C_DEV ?= i2c0
else ifeq ($(ARCH), stm32)
-ASPIRIN_I2C_DEV=i2c2
+ASPIRIN_I2C_DEV ?= i2c2
+endif
+
+ifeq ($(TARGET), ap)
+ifndef ASPIRIN_I2C_DEV
+$(error Error: ASPIRIN_I2C_DEV not configured!)
endif
endif
-# convert i2cx to upper case
+# convert i2cx to upper/lower case
ASPIRIN_I2C_DEV_UPPER=$(shell echo $(ASPIRIN_I2C_DEV) | tr a-z A-Z)
+ASPIRIN_I2C_DEV_LOWER=$(shell echo $(ASPIRIN_I2C_DEV) | tr A-Z a-z)
-IMU_ASPIRIN_CFLAGS += -DASPIRIN_I2C_DEV=$(ASPIRIN_I2C_DEV)
+IMU_ASPIRIN_CFLAGS += -DASPIRIN_I2C_DEV=$(ASPIRIN_I2C_DEV_LOWER)
IMU_ASPIRIN_CFLAGS += -DUSE_$(ASPIRIN_I2C_DEV_UPPER)
+
include $(CFG_SHARED)/imu_nps.makefile
diff --git a/conf/firmwares/subsystems/shared/imu_aspirin_v2.2.makefile b/conf/firmwares/subsystems/shared/imu_aspirin_v2.2.makefile
index f9198a182c..9a1633ad9f 100644
--- a/conf/firmwares/subsystems/shared/imu_aspirin_v2.2.makefile
+++ b/conf/firmwares/subsystems/shared/imu_aspirin_v2.2.makefile
@@ -47,7 +47,7 @@ include $(CFG_SHARED)/imu_aspirin_v2_common.makefile
# so that it will be unselected at init (baro CS line high)
#
ifeq ($(ARCH), lpc21)
-IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE1
+#IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE1
else ifeq ($(ARCH), stm32)
# SLAVE3 is on PC13, which is the baro CS
IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE3
diff --git a/conf/firmwares/subsystems/shared/imu_aspirin_v2_common.makefile b/conf/firmwares/subsystems/shared/imu_aspirin_v2_common.makefile
index 150a6f0bac..f8b3560ed5 100644
--- a/conf/firmwares/subsystems/shared/imu_aspirin_v2_common.makefile
+++ b/conf/firmwares/subsystems/shared/imu_aspirin_v2_common.makefile
@@ -50,18 +50,37 @@ IMU_ASPIRIN_2_SRCS += peripherals/mpu60x0_spi.c
include $(CFG_SHARED)/spi_master.makefile
+#
+# SPI device and slave select defaults
+#
ifeq ($(ARCH), lpc21)
-IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE0
-IMU_ASPIRIN_2_CFLAGS += -DASPIRIN_2_SPI_SLAVE_IDX=SPI_SLAVE0
-IMU_ASPIRIN_2_CFLAGS += -DASPIRIN_2_SPI_DEV=spi1
-IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI1
+ASPIRIN_2_SPI_DEV ?= spi1
+ASPIRIN_2_SPI_SLAVE_IDX ?= SPI_SLAVE0
else ifeq ($(ARCH), stm32)
-IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI2
# Slave select configuration
# SLAVE2 is on PB12 (NSS) (MPU600 CS)
-IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE2
+ASPIRIN_2_SPI_DEV ?= spi2
+ASPIRIN_2_SPI_SLAVE_IDX ?= SPI_SLAVE2
endif
+ifeq ($(TARGET), ap)
+ifndef ASPIRIN_2_SPI_DEV
+$(error Error: ASPIRIN_2_SPI_DEV not configured!)
+endif
+ifndef ASPIRIN_2_SPI_SLAVE_IDX
+$(error Error: ASPIRIN_2_SPI_SLAVE_IDX not configured!)
+endif
+endif
+
+ASPIRIN_2_SPI_DEV_UPPER=$(shell echo $(ASPIRIN_2_SPI_DEV) | tr a-z A-Z)
+ASPIRIN_2_SPI_DEV_LOWER=$(shell echo $(ASPIRIN_2_SPI_DEV) | tr A-Z a-z)
+
+IMU_ASPIRIN_2_CFLAGS += -DUSE_$(ASPIRIN_2_SPI_DEV_UPPER)
+IMU_ASPIRIN_2_CFLAGS += -DASPIRIN_2_SPI_DEV=$(ASPIRIN_2_SPI_DEV_LOWER)
+
+IMU_ASPIRIN_2_CFLAGS += -DUSE_$(ASPIRIN_2_SPI_SLAVE_IDX)
+IMU_ASPIRIN_2_CFLAGS += -DASPIRIN_2_SPI_SLAVE_IDX=$(ASPIRIN_2_SPI_SLAVE_IDX)
+
# Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets
# and re-use that in the imu_aspirin_v2.1 and imu_aspirin_v2.2 makefiles
diff --git a/conf/firmwares/subsystems/shared/imu_drotek_10dof_v2.makefile b/conf/firmwares/subsystems/shared/imu_drotek_10dof_v2.makefile
index b41ec9b741..17c50f6ba2 100644
--- a/conf/firmwares/subsystems/shared/imu_drotek_10dof_v2.makefile
+++ b/conf/firmwares/subsystems/shared/imu_drotek_10dof_v2.makefile
@@ -53,18 +53,21 @@ IMU_DROTEK_2_SRCS += peripherals/hmc58xx.c
# set default i2c bus
-ifndef DROTEK_2_I2C_DEV
ifeq ($(ARCH), lpc21)
-DROTEK_2_I2C_DEV=i2c0
+DROTEK_2_I2C_DEV ?= i2c0
else ifeq ($(ARCH), stm32)
-DROTEK_2_I2C_DEV=i2c2
-endif
+DROTEK_2_I2C_DEV ?= i2c2
endif
-# convert i2cx to upper case
+ifndef DROTEK_2_I2C_DEV
+$(error Error: DROTEK_2_I2C_DEV not configured!)
+endif
+
+# convert i2cx to upper/lower case
DROTEK_2_I2C_DEV_UPPER=$(shell echo $(DROTEK_2_I2C_DEV) | tr a-z A-Z)
+DROTEK_2_I2C_DEV_LOWER=$(shell echo $(DROTEK_2_I2C_DEV) | tr A-Z a-z)
-IMU_DROTEK_2_CFLAGS += -DDROTEK_2_I2C_DEV=$(DROTEK_2_I2C_DEV)
+IMU_DROTEK_2_CFLAGS += -DDROTEK_2_I2C_DEV=$(DROTEK_2_I2C_DEV_LOWER)
IMU_DROTEK_2_CFLAGS += -DUSE_$(DROTEK_2_I2C_DEV_UPPER)
diff --git a/conf/firmwares/subsystems/shared/imu_gl1.makefile b/conf/firmwares/subsystems/shared/imu_gl1.makefile
new file mode 100644
index 0000000000..18107c5c9f
--- /dev/null
+++ b/conf/firmwares/subsystems/shared/imu_gl1.makefile
@@ -0,0 +1,52 @@
+# Hey Emacs, this is a -*- makefile -*-
+#
+# IMU from Goodluckbuy
+#
+
+
+IMU_GL1_CFLAGS = -DIMU_TYPE_H=\"imu/imu_gl1.h\"
+IMU_GL1_SRCS = $(SRC_SUBSYSTEMS)/imu.c
+IMU_GL1_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_gl1.c
+
+# for fixedwing firmware and ap only
+ifeq ($(TARGET), ap)
+ IMU_GL1_CFLAGS += -DUSE_IMU
+endif
+
+# Accelerometer
+IMU_GL1_SRCS += peripherals/adxl345_i2c.c
+
+# Gyro
+IMU_GL1_SRCS += peripherals/l3g4200.c
+
+# Magnetometer
+IMU_GL1_SRCS += peripherals/hmc58xx.c
+
+ifeq ($(ARCH), lpc21)
+GL1_I2C_DEV ?= i2c0
+else ifeq ($(ARCH), stm32)
+GL1_I2C_DEV ?= i2c2
+endif
+
+ifndef GL1_I2C_DEV
+$(error Error: GL1_I2C_DEV not configured!)
+endif
+
+# convert i2cx to upper/lower case
+GL1_I2C_DEV_UPPER=$(shell echo $(GL1_I2C_DEV) | tr a-z A-Z)
+GL1_I2C_DEV_LOWER=$(shell echo $(GL1_I2C_DEV) | tr A-Z a-z)
+
+IMU_GL1_CFLAGS += -DGL1_I2C_DEV=$(GL1_I2C_DEV_LOWER)
+IMU_GL1_CFLAGS += -DUSE_$(GL1_I2C_DEV_UPPER)
+
+# Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets
+# see: conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.1.makefile for example
+ap.CFLAGS += $(IMU_GL1_CFLAGS)
+ap.srcs += $(IMU_GL1_SRCS)
+
+
+
+#
+# NPS simulator
+#
+include $(CFG_SHARED)/imu_nps.makefile
diff --git a/conf/firmwares/subsystems/shared/imu_krooz_sd.makefile b/conf/firmwares/subsystems/shared/imu_krooz_sd.makefile
index c2a7e83fe0..baf9c6ae10 100644
--- a/conf/firmwares/subsystems/shared/imu_krooz_sd.makefile
+++ b/conf/firmwares/subsystems/shared/imu_krooz_sd.makefile
@@ -14,11 +14,9 @@ IMU_KROOZ_I2C_DEV=i2c2
IMU_KROOZ_CFLAGS += -DUSE_I2C -DUSE_I2C2 -DI2C2_CLOCK_SPEED=400000
IMU_KROOZ_CFLAGS += -DIMU_KROOZ_I2C_DEV=$(IMU_KROOZ_I2C_DEV)
-IMU_KROOZ_CFLAGS += -DMS5611_I2C_DEV=$(IMU_KROOZ_I2C_DEV) -DMS5611_SLAVE_ADDR=0xEC
IMU_KROOZ_SRCS += peripherals/mpu60x0.c
IMU_KROOZ_SRCS += peripherals/mpu60x0_i2c.c
IMU_KROOZ_SRCS += peripherals/hmc58xx.c
-IMU_KROOZ_SRCS += modules/sensors/baro_ms5611_i2c.c
AHRS_PROPAGATE_FREQUENCY ?= 512
AHRS_CORRECT_FREQUENCY ?= 512
@@ -27,3 +25,8 @@ ap.CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY)
ap.CFLAGS += $(IMU_KROOZ_CFLAGS)
ap.srcs += $(IMU_KROOZ_SRCS)
+
+#
+# NPS simulator
+#
+include $(CFG_SHARED)/imu_nps.makefile
diff --git a/conf/firmwares/subsystems/shared/imu_lisa_s_v0.1.makefile b/conf/firmwares/subsystems/shared/imu_lisa_s_v0.1.makefile
new file mode 100644
index 0000000000..7f75e5c3a9
--- /dev/null
+++ b/conf/firmwares/subsystems/shared/imu_lisa_s_v0.1.makefile
@@ -0,0 +1,70 @@
+# Hey Emacs, this is a -*- makefile -*-
+#
+# Common part for Aspirin IMU v2.1 and v2.2
+#
+#
+# required xml:
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+
+
+# for fixedwing firmware and ap only
+ifeq ($(TARGET), ap)
+ IMU_ASPIRIN_2_CFLAGS = -DUSE_IMU
+endif
+
+IMU_ASPIRIN_2_CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin_2_spi.h\"
+IMU_ASPIRIN_2_SRCS = $(SRC_SUBSYSTEMS)/imu.c
+IMU_ASPIRIN_2_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_aspirin_2_spi.c
+IMU_ASPIRIN_2_SRCS += peripherals/mpu60x0.c
+IMU_ASPIRIN_2_SRCS += peripherals/mpu60x0_spi.c
+
+include $(CFG_SHARED)/spi_master.makefile
+
+IMU_ASPIRIN_2_CFLAGS += -DASPIRIN_2_SPI_SLAVE_IDX=SPI_SLAVE0
+IMU_ASPIRIN_2_CFLAGS += -DASPIRIN_2_SPI_DEV=spi1
+
+IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI1 -DLISA_S
+# Slave select configuration
+# SLAVE0 is on PA4 (NSS) (MPU600 CS)
+IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE0
+
+# SLAVE1 is on PC4, which is the baro CS
+IMU_ASPIRIN_2_CFLAGS += -DUSE_SPI_SLAVE1
+
+ap.CFLAGS += $(IMU_ASPIRIN_2_CFLAGS)
+ap.srcs += $(IMU_ASPIRIN_2_SRCS)
+
+#
+# NPS simulator
+#
+include $(CFG_SHARED)/imu_nps.makefile
diff --git a/conf/firmwares/subsystems/shared/imu_nps.makefile b/conf/firmwares/subsystems/shared/imu_nps.makefile
index e26d035cd4..a84003b6d6 100644
--- a/conf/firmwares/subsystems/shared/imu_nps.makefile
+++ b/conf/firmwares/subsystems/shared/imu_nps.makefile
@@ -20,5 +20,5 @@
#
#
-nps.CFLAGS += -DIMU_TYPE_H=\"imu/imu_nps.h\"
+nps.CFLAGS += -DIMU_TYPE_H=\"imu/imu_nps.h\" -DUSE_IMU
nps.srcs += $(SRC_SUBSYSTEMS)/imu.c $(SRC_SUBSYSTEMS)/imu/imu_nps.c
diff --git a/conf/firmwares/subsystems/shared/imu_px4fmu_v1.7.makefile b/conf/firmwares/subsystems/shared/imu_px4fmu_v1.7.makefile
new file mode 100644
index 0000000000..68a7bc0099
--- /dev/null
+++ b/conf/firmwares/subsystems/shared/imu_px4fmu_v1.7.makefile
@@ -0,0 +1,70 @@
+# Hey Emacs, this is a -*- makefile -*-
+#
+# MPU6000 via SPI and HMC5883 via I2C on the PX4FMU v1.7 board
+#
+#
+# required xml:
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+#
+
+
+# for fixedwing firmware and ap only
+ifeq ($(TARGET), ap)
+ IMU_PX4FMU_CFLAGS = -DUSE_IMU
+endif
+
+include $(CFG_SHARED)/spi_master.makefile
+
+IMU_PX4FMU_CFLAGS += -DIMU_TYPE_H=\"imu/imu_px4fmu.h\"
+IMU_PX4FMU_SRCS = $(SRC_SUBSYSTEMS)/imu.c
+IMU_PX4FMU_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_px4fmu.c
+IMU_PX4FMU_SRCS += peripherals/mpu60x0.c
+IMU_PX4FMU_SRCS += peripherals/mpu60x0_spi.c
+IMU_PX4FMU_CFLAGS += -DUSE_SPI1
+IMU_PX4FMU_CFLAGS += -DUSE_SPI_SLAVE0 -DUSE_SPI_SLAVE1 -DUSE_SPI_SLAVE2
+
+# Magnetometer
+IMU_PX4FMU_SRCS += peripherals/hmc58xx.c
+IMU_PX4FMU_CFLAGS += -DUSE_I2C2
+
+
+# Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets
+# see: conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.1.makefile for example
+
+ap.CFLAGS += $(IMU_PX4FMU_CFLAGS)
+ap.srcs += $(IMU_PX4FMU_SRCS)
+
+
+#
+# NPS simulator
+#
+include $(CFG_SHARED)/imu_nps.makefile
diff --git a/conf/firmwares/subsystems/shared/radio_control_datalink.makefile b/conf/firmwares/subsystems/shared/radio_control_datalink.makefile
index bb342a5c3e..d24c413b5a 100644
--- a/conf/firmwares/subsystems/shared/radio_control_datalink.makefile
+++ b/conf/firmwares/subsystems/shared/radio_control_datalink.makefile
@@ -9,11 +9,17 @@ ifeq ($(BOARD),classix)
endif
endif
+RADIO_CONTROL_DATALINK_LED ?= none
+RADIO_CONTROL_LED ?= none
ifeq ($(NORADIO), False)
-ifdef (RADIO_CONTROL_DATALINK_LED)
- ap.CFLAGS += -D(RADIO_CONTROL_DATALINK_LED=$((RADIO_CONTROL_DATALINK_LED)
-endif
+ ifneq ($(RADIO_CONTROL_DATALINK_LED),none)
+ ap.CFLAGS += -DRADIO_CONTROL_DATALINK_LED=$(RADIO_CONTROL_DATALINK_LED)
+ endif
+ ifneq ($(RADIO_CONTROL_LED),none)
+ ap.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED)
+ fbw.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED)
+ endif
$(TARGET).CFLAGS += -DRADIO_CONTROL
$(TARGET).CFLAGS += -DRADIO_CONTROL_TYPE_H=\"radio_control/rc_datalink.h\"
$(TARGET).CFLAGS += -DRADIO_CONTROL_TYPE_DATALINK
diff --git a/conf/firmwares/subsystems/shared/radio_control_ppm.makefile b/conf/firmwares/subsystems/shared/radio_control_ppm.makefile
index f8e49d96d5..19801b6e74 100644
--- a/conf/firmwares/subsystems/shared/radio_control_ppm.makefile
+++ b/conf/firmwares/subsystems/shared/radio_control_ppm.makefile
@@ -3,6 +3,7 @@
#
NORADIO = False
+RADIO_CONTROL_LED ?= none
ifeq ($(BOARD),classix)
ifeq ($(TARGET),ap)
diff --git a/conf/firmwares/subsystems/shared/radio_control_sbus.makefile b/conf/firmwares/subsystems/shared/radio_control_sbus.makefile
index 2e8e9ecbe1..67970f298a 100644
--- a/conf/firmwares/subsystems/shared/radio_control_sbus.makefile
+++ b/conf/firmwares/subsystems/shared/radio_control_sbus.makefile
@@ -2,11 +2,14 @@
# Makefile for shared radio_control SBUS subsystem
#
-$(TARGET).CFLAGS += -DRADIO_CONTROL
+RADIO_CONTROL_LED ?= none
+
ifneq ($(RADIO_CONTROL_LED),none)
ap.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED)
endif
+$(TARGET).CFLAGS += -DRADIO_CONTROL
+
# convert SBUS_PORT to upper and lower case strings:
SBUS_PORT_UPPER=$(shell echo $(SBUS_PORT) | tr a-z A-Z)
SBUS_PORT_LOWER=$(shell echo $(SBUS_PORT) | tr A-Z a-z)
diff --git a/conf/firmwares/subsystems/shared/radio_control_spektrum.makefile b/conf/firmwares/subsystems/shared/radio_control_spektrum.makefile
index 05d9d555c4..6e92d28d91 100644
--- a/conf/firmwares/subsystems/shared/radio_control_spektrum.makefile
+++ b/conf/firmwares/subsystems/shared/radio_control_spektrum.makefile
@@ -1,6 +1,9 @@
#
# Makefile for shared radio_control spektrum susbsytem
#
+
+RADIO_CONTROL_LED ?= none
+
ifndef RADIO_CONTROL_SPEKTRUM_MODEL
RADIO_CONTROL_SPEKTRUM_MODEL=\"subsystems/radio_control/spektrum_dx7se.h\"
endif
diff --git a/conf/firmwares/subsystems/shared/radio_control_superbitrf_rc.makefile b/conf/firmwares/subsystems/shared/radio_control_superbitrf_rc.makefile
new file mode 100644
index 0000000000..218a7c194c
--- /dev/null
+++ b/conf/firmwares/subsystems/shared/radio_control_superbitrf_rc.makefile
@@ -0,0 +1,17 @@
+#
+# Makefile for shared radio_control superbitrf subsystem
+#
+
+RADIO_CONTROL_LED ?= none
+
+ap.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_TYPE_SUPERBITRF -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/superbitrf_rc.h\"
+ap.CFLAGS += -DUSE_SUPERBITRF -DUSE_SPI2 -DUSE_SPI_SLAVE2
+
+ifneq ($(RADIO_CONTROL_LED),none)
+ap.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED)
+endif
+
+ap.srcs += peripherals/cyrf6936.c \
+ $(SRC_SUBSYSTEMS)/datalink/superbitrf.c\
+ $(SRC_SUBSYSTEMS)/radio_control.c \
+ $(SRC_SUBSYSTEMS)/radio_control/superbitrf_rc.c
diff --git a/conf/flight_plans/rotorcraft_basic.xml b/conf/flight_plans/rotorcraft_basic.xml
index e9798d0de1..de897364e2 100644
--- a/conf/flight_plans/rotorcraft_basic.xml
+++ b/conf/flight_plans/rotorcraft_basic.xml
@@ -71,8 +71,12 @@
+
+
+
+
diff --git a/conf/flight_plans/rotorcraft_basic_superbitrf.xml b/conf/flight_plans/rotorcraft_basic_superbitrf.xml
new file mode 100644
index 0000000000..635610b6ae
--- /dev/null
+++ b/conf/flight_plans/rotorcraft_basic_superbitrf.xml
@@ -0,0 +1,113 @@
+
+
+
+
+#include "autopilot.h"
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/flight_plans/versatile.xml b/conf/flight_plans/versatile.xml
index dcf3d90931..f08147adaa 100644
--- a/conf/flight_plans/versatile.xml
+++ b/conf/flight_plans/versatile.xml
@@ -1,6 +1,6 @@
-
+
#include "subsystems/datalink/datalink.h"
diff --git a/conf/flight_plans/versatile_airspeed.xml b/conf/flight_plans/versatile_airspeed.xml
index 1427adf72c..d98264c6e6 100644
--- a/conf/flight_plans/versatile_airspeed.xml
+++ b/conf/flight_plans/versatile_airspeed.xml
@@ -1,6 +1,6 @@
-
+
#include "firmwares/fixedwing/guidance/energy_ctrl.h"
#include "subsystems/datalink/datalink.h"
diff --git a/conf/flight_plans/zamboni_survey_test.xml b/conf/flight_plans/zamboni_survey_test.xml
new file mode 100644
index 0000000000..c8d138668f
--- /dev/null
+++ b/conf/flight_plans/zamboni_survey_test.xml
@@ -0,0 +1,82 @@
+
+
+
+
+#include "subsystems/datalink/datalink.h"
+#include "modules/digital_cam/dc.h"
+//#define LINE_START_FUNCTION dc_autoshoot = DC_AUTOSHOOT_DISTANCE;
+//#define LINE_START_FUNCTION dc_Survey(dc_gps_dist);
+#define LINE_START_FUNCTION dc_Survey(40);
+#define LINE_STOP_FUNCTION dc_autoshoot = DC_AUTOSHOOT_STOP;
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/maps.xml.example b/conf/maps_example.xml
similarity index 100%
rename from conf/maps.xml.example
rename to conf/maps_example.xml
diff --git a/conf/messages.xml b/conf/messages.xml
index 2ddf6a8999..435a83c755 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -43,11 +43,12 @@
-
+
+
@@ -255,6 +256,8 @@
+
+
@@ -578,10 +581,10 @@
-
-
-
-
+
+
+
+
@@ -601,11 +604,45 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -1828,7 +1865,13 @@
-
+
+
+
+
+
+
+
@@ -2067,6 +2110,7 @@
+
diff --git a/conf/modules/AOA_adc.xml b/conf/modules/AOA_adc.xml
index fd42c505be..b9338b2f67 100644
--- a/conf/modules/AOA_adc.xml
+++ b/conf/modules/AOA_adc.xml
@@ -10,15 +10,13 @@
-
-
+
+
-
-
-
+
diff --git a/conf/modules/baro_bmp.xml b/conf/modules/baro_bmp.xml
index 0fa9cc1edf..1bf6bde874 100644
--- a/conf/modules/baro_bmp.xml
+++ b/conf/modules/baro_bmp.xml
@@ -10,10 +10,11 @@
-
+
-
+
+
diff --git a/conf/modules/baro_ms5611_i2c.xml b/conf/modules/baro_ms5611_i2c.xml
index 412c725338..027cd4061c 100644
--- a/conf/modules/baro_ms5611_i2c.xml
+++ b/conf/modules/baro_ms5611_i2c.xml
@@ -13,12 +13,13 @@
-
-
-
+
+
+
+
diff --git a/conf/modules/baro_ms5611_spi.xml b/conf/modules/baro_ms5611_spi.xml
new file mode 100644
index 0000000000..2ec153e554
--- /dev/null
+++ b/conf/modules/baro_ms5611_spi.xml
@@ -0,0 +1,25 @@
+
+
+
+
+
+ Baro MS5611 (SPI)
+ Measurement Specialties MS5611-01BA pressure sensor (SPI)
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/flight_time.xml b/conf/modules/flight_time.xml
new file mode 100644
index 0000000000..46ab86b367
--- /dev/null
+++ b/conf/modules/flight_time.xml
@@ -0,0 +1,18 @@
+
+
+
+
+
+ Flight time calculator
+ Allows to check how much time is left before the end of the competition.
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/conf/modules/gumstix_qr_code_spi_link.xml b/conf/modules/gumstix_qr_code_spi_link.xml
new file mode 100644
index 0000000000..6219c94a66
--- /dev/null
+++ b/conf/modules/gumstix_qr_code_spi_link.xml
@@ -0,0 +1,19 @@
+
+
+
+
+ QR code gumstix interface
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/light.xml b/conf/modules/light.xml
index 33cfd74154..5761db11f1 100644
--- a/conf/modules/light.xml
+++ b/conf/modules/light.xml
@@ -15,7 +15,7 @@
-
+
+
+
+ 0.00085
+ 18.0
+ 2
+ 30
+ 30
+
+
+
+ 0.0 0.0776
+ 0.1 0.0744
+ 0.2 0.0712
+ 0.3 0.0655
+ 0.4 0.0588
+ 0.5 0.0518
+ 0.6 0.0419
+ 0.7 0.0318
+ 0.8 0.0172
+ 1.0 -0.0058
+ 1.4 -0.0549
+
+
+
+
+
+ 0.0 0.0902
+ 0.1 0.0893
+ 0.2 0.0880
+ 0.3 0.0860
+ 0.4 0.0810
+ 0.5 0.0742
+ 0.6 0.0681
+ 0.7 0.0572
+ 0.8 0.0467
+ 1.0 0.0167
+ 1.4 -0.0803
+
+
+
+
diff --git a/conf/simulator/jsbsim/aircraft/Engines/Zenoah_G-26A.xml b/conf/simulator/jsbsim/aircraft/Engines/Zenoah_G-26A.xml
new file mode 100644
index 0000000000..86d43a827e
--- /dev/null
+++ b/conf/simulator/jsbsim/aircraft/Engines/Zenoah_G-26A.xml
@@ -0,0 +1,8 @@
+
+
+
+
+
+
+ 2207.27
+
diff --git a/conf/simulator/jsbsim/aircraft/LisaAspirin2.xml b/conf/simulator/jsbsim/aircraft/LisaAspirin2.xml
new file mode 100644
index 0000000000..b3dafa86fc
--- /dev/null
+++ b/conf/simulator/jsbsim/aircraft/LisaAspirin2.xml
@@ -0,0 +1,546 @@
+
+
+
+
+
+ Author Name
+ Creation Date
+ Version
+ Models a Malolo
+
+
+
+ 10.57
+ 9.17
+ 1.15
+ 1.69
+ 3.28
+ 1.06
+ 0
+
+ 37.4
+ 0
+ 0
+
+
+ 20
+ 0
+ 5
+
+
+ 0
+ 0
+ 0
+
+
+
+
+ 1
+ 1
+ 2
+ 0
+ 0
+ 0
+ 12
+
+ 36.4
+ 0
+ 4
+
+
+ 1
+
+ 0
+ 0
+ 0
+
+
+
+
+
+
+
+ 40.1
+ -9.9
+ -10.1
+
+ 0.8
+ 0.5
+ 0.02
+ 120
+ 20
+ 0.0
+ LEFT
+ 0
+
+
+
+ 40.1
+ 9.9
+ -10.1
+
+ 0.8
+ 0.5
+ 0.02
+ 120
+ 20
+ 0.0
+ RIGHT
+ 0
+
+
+
+ 68.9
+ 0
+ -4.3
+
+ 0.8
+ 0.5
+ 0.02
+ 24
+ 20
+ 360.0
+ NONE
+ 0
+
+
+
+ 10
+ 0
+ -8.3
+
+ 0.8
+ 0.5
+ 0.02
+ 24
+ 20
+ 360.0
+ NONE
+ 0
+
+
+
+
+
+
+
+ 36
+ 0
+ 0
+
+
+ 0.0
+ 0
+ 0
+
+ 0
+
+
+ 1
+ 0
+ 0
+
+
+ 0.0
+ 0.0
+ 0.0
+
+ 1.0
+
+
+
+
+ 36.36
+ 0
+ -1.89375
+
+ 1.5
+ 1.5
+
+
+
+
+
+
+
+ fcs/elevator-cmd-norm
+ fcs/pitch-trim-cmd-norm
+
+ -1
+ 1
+
+
+
+
+ fcs/pitch-trim-sum
+
+ -0.35
+ 0.3
+
+
+
+
+
+ fcs/elevator-pos-rad
+
+ -0.3
+ 0.3
+
+
+ -1
+ 1
+
+
+
+
+
+ fcs/aileron-cmd-norm
+ fcs/roll-trim-cmd-norm
+
+ -1
+ 1
+
+
+
+
+ fcs/roll-trim-sum
+
+ -0.35
+ 0.35
+
+
+
+
+
+ -fcs/roll-trim-sum
+
+ -0.35
+ 0.35
+
+
+
+
+
+ fcs/left-aileron-pos-rad
+
+ -0.35
+ 0.35
+
+
+ -1
+ 1
+
+
+
+
+
+ fcs/right-aileron-pos-rad
+
+ -0.35
+ 0.35
+
+
+ -1
+ 1
+
+
+
+
+
+ fcs/rudder-cmd-norm
+ fcs/yaw-trim-cmd-norm
+
+ -1
+ 1
+
+
+
+
+ fcs/rudder-command-sum
+
+ -0.35
+ 0.35
+
+
+
+
+
+ fcs/rudder-pos-rad
+
+ -0.35
+ 0.35
+
+
+ -1
+ 1
+
+
+
+
+
+
+
+
+
+ Drag_at_zero_lift
+
+ aero/qbar-psf
+ metrics/Sw-sqft
+
+ aero/alpha-rad
+
+ -1.5700 1.5000
+ -0.2600 0.0560
+ 0.0000 0.0280
+ 0.2600 0.0560
+ 1.5700 1.5000
+
+
+
+
+
+ Induced_drag
+
+ aero/qbar-psf
+ metrics/Sw-sqft
+ aero/cl-squared
+ 0.0400
+
+
+
+ Drag_due_to_sideslip
+
+ aero/qbar-psf
+ metrics/Sw-sqft
+
+ aero/beta-rad
+
+ -1.5700 1.2300
+ -0.2600 0.0500
+ 0.0000 0.0000
+ 0.2600 0.0500
+ 1.5700 1.2300
+
+
+
+
+
+ Drag_due_to_Elevator_Deflection
+
+ aero/qbar-psf
+ metrics/Sw-sqft
+ fcs/elevator-pos-norm
+ 0.0300
+
+
+
+
+
+
+ Side_force_due_to_beta
+
+ aero/qbar-psf
+ metrics/Sw-sqft
+ aero/beta-rad
+ -1.0000
+
+
+
+
+
+
+ Lift_due_to_alpha
+
+ aero/qbar-psf
+ metrics/Sw-sqft
+
+ aero/alpha-rad
+
+ -0.2000 -0.7500
+ 0.0000 0.2500
+ 0.2300 1.4000
+ 0.6000 0.7100
+
+
+
+
+
+ Lift_due_to_Elevator_Deflection
+
+ aero/qbar-psf
+ metrics/Sw-sqft
+ fcs/elevator-pos-rad
+ 0.2000
+
+
+
+
+
+
+ Roll_moment_due_to_beta
+
+
+ aero/qbar-psf
+ metrics/Sw-sqft
+ metrics/bw-ft
+ aero/beta-rad
+ -0.1000
+
+
+
+ Roll_moment_due_to_roll_rate
+
+ aero/qbar-psf
+ metrics/Sw-sqft
+ metrics/bw-ft
+ aero/bi2vel
+ velocities/p-aero-rad_sec
+ -0.4000
+
+
+
+ Roll_moment_due_to_yaw_rate
+
+ aero/qbar-psf
+ metrics/Sw-sqft
+ metrics/bw-ft
+ aero/bi2vel
+ velocities/r-aero-rad_sec
+ 0.1500
+
+
+
+ Roll_moment_due_to_aileron
+
+ aero/qbar-psf
+ metrics/Sw-sqft
+ metrics/bw-ft
+ fcs/left-aileron-pos-rad
+
+ velocities/mach
+
+ 0.0000 0.1300
+ 2.0000 0.0570
+
+
+
+
+
+ Roll_moment_due_to_rudder
+
+ aero/qbar-psf
+ metrics/Sw-sqft
+ metrics/bw-ft
+ fcs/rudder-pos-rad
+ 0.0100
+
+
+
+
+
+
+ Pitch_moment_due_to_alpha
+
+ aero/qbar-psf
+ metrics/Sw-sqft
+ metrics/cbarw-ft
+ aero/alpha-rad
+ -0.5000
+
+
+
+ Pitch_moment_due_to_elevator
+
+ aero/qbar-psf
+ metrics/Sw-sqft
+ metrics/cbarw-ft
+ fcs/elevator-pos-rad
+
+ velocities/mach
+
+ 0.0000 -0.5000
+ 2.0000 -0.2750
+
+
+
+
+
+ Pitch_moment_due_to_pitch_rate
+
+ aero/qbar-psf
+ metrics/Sw-sqft
+ metrics/cbarw-ft
+ aero/ci2vel
+ velocities/q-aero-rad_sec
+ -12.0000
+
+
+
+ Pitch_moment_due_to_alpha_rate
+
+ aero/qbar-psf
+ metrics/Sw-sqft
+ metrics/cbarw-ft
+ aero/ci2vel
+ aero/alphadot-rad_sec
+ -7.0000
+
+
+
+
+
+
+ Yaw_moment_due_to_beta
+
+ aero/qbar-psf
+ metrics/Sw-sqft
+ metrics/bw-ft
+ aero/beta-rad
+ 0.1200
+
+
+
+ Yaw_moment_due_to_yaw_rate
+
+ aero/qbar-psf
+ metrics/Sw-sqft
+ metrics/bw-ft
+ aero/bi2vel
+ velocities/r-aero-rad_sec
+ -0.1500
+
+
+
+ Yaw_moment_due_to_rudder
+
+ aero/qbar-psf
+ metrics/Sw-sqft
+ metrics/bw-ft
+ fcs/rudder-pos-rad
+ -0.0500
+
+
+
+ Adverse_yaw
+
+ aero/qbar-psf
+ metrics/Sw-sqft
+ metrics/bw-ft
+ fcs/left-aileron-pos-rad
+ -0.0300
+
+
+
+ Yaw_moment_due_to_tail_incidence
+
+ aero/qbar-psf
+ metrics/Sw-sqft
+ metrics/bw-ft
+ 0.0007
+
+
+
+
+
diff --git a/conf/system/udev/rules/50-paparazzi.rules b/conf/system/udev/rules/50-paparazzi.rules
index 4a8f7ef37e..6d50b0b908 100644
--- a/conf/system/udev/rules/50-paparazzi.rules
+++ b/conf/system/udev/rules/50-paparazzi.rules
@@ -27,6 +27,9 @@ ATTRS{idVendor}=="0403", ATTRS{idProduct}=="cff8", GROUP="plugdev"
# FTDI 2232 based jtag for Lisa/L and usb upload
ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6010", MODE="0666", GROUP="plugdev"
+# dfu devices
+ATTRS{idVendor}=="0483", ATTRS{idProduct}=="df11", MODE="0666", GROUP="plugdev"
+
# all (fake VID 0x7070) LPCUSB devices (access through libusb)
ATTRS{idVendor}=="7070", GROUP="plugdev"
diff --git a/conf/telemetry/default_rotorcraft.xml b/conf/telemetry/default_rotorcraft.xml
index 6f83d9b340..2a79fd2c04 100644
--- a/conf/telemetry/default_rotorcraft.xml
+++ b/conf/telemetry/default_rotorcraft.xml
@@ -18,6 +18,7 @@
+
diff --git a/data/maps/Makefile b/data/maps/Makefile
index 03e3fcb5ef..e13be3e9e8 100644
--- a/data/maps/Makefile
+++ b/data/maps/Makefile
@@ -21,7 +21,7 @@ $(DATADIR):
$(DATADIR)/maps.google.com: $(DATADIR) FORCE
@echo "-----------------------------------------------"
@echo "DOWNLOAD: google maps version code";
- $(Q)wget -q -O $(@) http://maps.google.com/ || \
+ $(Q)wget -q -t 1 -T 10 -O $(@) http://maps.google.com/ || \
(rm -f $(@) && \
echo "Could not download google maps version code" && \
echo "-----------------------------------------------" && \
diff --git a/make-release-tarball.sh b/make-release-tarball.sh
new file mode 100755
index 0000000000..5d50033178
--- /dev/null
+++ b/make-release-tarball.sh
@@ -0,0 +1,255 @@
+#!/bin/bash -
+#
+# File: git-archive-all.sh
+#
+# Description: A utility script that builds an archive file(s) of all
+# git repositories and submodules in the current path.
+# Useful for creating a single tarfile of a git super-
+# project that contains other submodules.
+#
+# Examples: Use git-archive-all.sh to create archive distributions
+# from git repositories. To use, simply do:
+#
+# cd $GIT_DIR; git-archive-all.sh
+#
+# where $GIT_DIR is the root of your git superproject.
+#
+# License: GPL3
+#
+###############################################################################
+#
+# This program is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+#
+###############################################################################
+
+# DEBUGGING
+set -e
+set -C # noclobber
+
+# TRAP SIGNALS
+trap 'cleanup' QUIT EXIT
+
+# For security reasons, explicitly set the internal field separator
+# to newline, space, tab
+OLD_IFS=$IFS
+IFS='
+ '
+
+function cleanup () {
+ rm -f $TMPFILE
+ rm -f $TOARCHIVE
+ IFS="$OLD_IFS"
+}
+
+function usage () {
+ echo "Usage is as follows:"
+ echo
+ echo "$PROGRAM <--version>"
+ echo " Prints the program version number on a line by itself and exits."
+ echo
+ echo "$PROGRAM <--usage|--help|-?>"
+ echo " Prints this usage output and exits."
+ echo
+ echo "$PROGRAM [--format ] [--prefix ] [--verbose|-v] [--separate|-s] [output_file]"
+ echo " Creates an archive for the entire git superproject, and its submodules"
+ echo " using the passed parameters, described below."
+ echo
+ echo " If '--format' is specified, the archive is created with the named"
+ echo " git archiver backend. Obviously, this must be a backend that git archive"
+ echo " understands. The format defaults to 'tar' if not specified."
+ echo
+ echo " If '--prefix' is specified, the archive's superproject and all submodules"
+ echo " are created with the prefix named. The default is to not use one."
+ echo
+ echo " If '--separate' or '-s' is specified, individual archives will be created"
+ echo " for each of the superproject itself and its submodules. The default is to"
+ echo " concatenate individual archives into one larger archive."
+ echo
+ echo " If 'output_file' is specified, the resulting archive is created as the"
+ echo " file named. This parameter is essentially a path that must be writeable."
+ echo " When combined with '--separate' ('-s') this path must refer to a directory."
+ echo " Without this parameter or when combined with '--separate' the resulting"
+ echo " archive(s) are named with a dot-separated path of the archived directory and"
+ echo " a file extension equal to their format (e.g., 'superdir.submodule1dir.tar')."
+ echo
+ echo " If '--verbose' or '-v' is specified, progress will be printed."
+}
+
+function version () {
+ echo "$PROGRAM version $VERSION"
+}
+
+# Internal variables and initializations.
+readonly PROGRAM=`basename "$0"`
+readonly VERSION=0.2
+
+OLD_PWD="`pwd`"
+TMPDIR=${TMPDIR:-/tmp}
+TMPFILE=`mktemp "$TMPDIR/$PROGRAM.XXXXXX"` # Create a place to store our work's progress
+TOARCHIVE=`mktemp "$TMPDIR/$PROGRAM.toarchive.XXXXXX"`
+OUT_FILE=$OLD_PWD # assume "this directory" without a name change by default
+SEPARATE=0
+VERBOSE=0
+
+FORMAT=tar
+PREFIX=
+TREEISH=HEAD
+
+# RETURN VALUES/EXIT STATUS CODES
+readonly E_BAD_OPTION=254
+readonly E_UNKNOWN=255
+
+# Process command-line arguments.
+while test $# -gt 0; do
+ case $1 in
+ --format )
+ shift
+ FORMAT="$1"
+ shift
+ ;;
+
+ --prefix )
+ shift
+ PREFIX="$1"
+ shift
+ ;;
+
+ --separate | -s )
+ shift
+ SEPARATE=1
+ ;;
+
+ --version )
+ version
+ exit
+ ;;
+
+ --verbose | -v )
+ shift
+ VERBOSE=1
+ ;;
+
+ -? | --usage | --help )
+ usage
+ exit
+ ;;
+
+ -* )
+ echo "Unrecognized option: $1" >&2
+ usage
+ exit $E_BAD_OPTION
+ ;;
+
+ * )
+ break
+ ;;
+ esac
+done
+
+if [ ! -z "$1" ]; then
+ OUT_FILE="$1"
+ shift
+fi
+
+# Validate parameters; error early, error often.
+if [ $SEPARATE -eq 1 -a ! -d $OUT_FILE ]; then
+ echo "When creating multiple archives, your destination must be a directory."
+ echo "If it's not, you risk being surprised when your files are overwritten."
+ exit
+elif [ `git config -l | grep -q '^core\.bare=false'; echo $?` -ne 0 ]; then
+ echo "$PROGRAM must be run from a git working copy (i.e., not a bare repository)."
+ exit
+fi
+
+# Create the superproject's git-archive
+if [ $VERBOSE -eq 1 ]; then
+ echo -n "creating superproject archive..."
+fi
+git archive --format=$FORMAT --prefix="$PREFIX" $TREEISH > $TMPDIR/$(basename $(pwd)).$FORMAT
+if [ $VERBOSE -eq 1 ]; then
+ echo "done"
+fi
+echo $TMPDIR/$(basename $(pwd)).$FORMAT >| $TMPFILE # clobber on purpose
+superfile=`head -n 1 $TMPFILE`
+
+if [ $VERBOSE -eq 1 ]; then
+ echo -n "looking for subprojects..."
+fi
+# find all '.git' dirs, these show us the remaining to-be-archived dirs
+# we only want directories that are below the current directory
+find . -mindepth 2 -name '.git' -type d -print | sed -e 's/^\.\///' -e 's/\.git$//' >> $TOARCHIVE
+# as of version 1.7.8, git places the submodule .git directories under the superprojects .git dir
+# the submodules get a .git file that points to their .git dir. we need to find all of these too
+find . -mindepth 2 -name '.git' -type f -print | xargs grep -l "gitdir" | sed -e 's/^\.\///' -e 's/\.git$//' >> $TOARCHIVE
+if [ $VERBOSE -eq 1 ]; then
+ echo "done"
+ echo " found:"
+ cat $TOARCHIVE | while read arch
+ do
+ echo " $arch"
+ done
+fi
+
+if [ $VERBOSE -eq 1 ]; then
+ echo -n "archiving submodules..."
+fi
+while read path; do
+ TREEISH=$(git submodule | grep "^ .*${path%/} " | cut -d ' ' -f 2) # git submodule does not list trailing slashes in $path
+ cd "$path"
+ git archive --format=$FORMAT --prefix="${PREFIX}$path" ${TREEISH:-HEAD} > "$TMPDIR"/"$(echo "$path" | sed -e 's/\//./g')"$FORMAT
+ if [ $FORMAT == 'zip' ]; then
+ # delete the empty directory entry; zipped submodules won't unzip if we don't do this
+ zip -d "$(tail -n 1 $TMPFILE)" "${PREFIX}${path%/}" >/dev/null # remove trailing '/'
+ fi
+ echo "$TMPDIR"/"$(echo "$path" | sed -e 's/\//./g')"$FORMAT >> $TMPFILE
+ cd "$OLD_PWD"
+done < $TOARCHIVE
+if [ $VERBOSE -eq 1 ]; then
+ echo "done"
+fi
+
+if [ $VERBOSE -eq 1 ]; then
+ echo -n "concatenating archives into single archive..."
+fi
+# Concatenate archives into a super-archive.
+if [ $SEPARATE -eq 0 ]; then
+ if [ $FORMAT == 'tar' ]; then
+ sed -e '1d' $TMPFILE | while read file; do
+ tar --concatenate -f "$superfile" "$file" && rm -f "$file"
+ done
+ elif [ $FORMAT == 'zip' ]; then
+ sed -e '1d' $TMPFILE | while read file; do
+ # zip incorrectly stores the full path, so cd and then grow
+ cd `dirname "$file"`
+ zip -g "$superfile" `basename "$file"` && rm -f "$file"
+ done
+ cd "$OLD_PWD"
+ fi
+
+ echo "$superfile" >| $TMPFILE # clobber on purpose
+fi
+if [ $VERBOSE -eq 1 ]; then
+ echo "done"
+fi
+
+if [ $VERBOSE -eq 1 ]; then
+ echo -n "moving archive to $OUT_FILE..."
+fi
+while read file; do
+ mv "$file" "$OUT_FILE"
+done < $TMPFILE
+if [ $VERBOSE -eq 1 ]; then
+ echo "done"
+fi
diff --git a/paparazzi_version b/paparazzi_version
index 0fde2c0289..6b01cf7563 100755
--- a/paparazzi_version
+++ b/paparazzi_version
@@ -1,12 +1,18 @@
#!/bin/sh
-DEF_VER=v5.0.0_stable
+DEF_VER=v5.1_devel
# First try git describe (if running on a git repo),
# then use default version from above (for release tarballs).
if test -d .git -o -f .git
then
- VN=$(git describe --match "v[0-9].[0-9]*" --dirty --always --long)
+ GIT_VN=$(git describe --match "v[0-9].[0-9]*" --dirty --always --long)
+ if echo "$GIT_VN" | grep -Eq "^v[0-9].[0-9]"
+ then
+ VN="$GIT_VN"
+ else
+ VN="$DEF_VER"-none-g"$GIT_VN"
+ fi
else
VN="$DEF_VER"
fi
diff --git a/select_conf.py b/select_conf.py
new file mode 100755
index 0000000000..08e91c004a
--- /dev/null
+++ b/select_conf.py
@@ -0,0 +1,262 @@
+#!/usr/bin/env python
+
+from __future__ import print_function
+
+import pygtk
+import gtk
+pygtk.require('2.0')
+
+
+import os
+import shutil
+import datetime
+from fnmatch import fnmatch
+import subprocess
+
+
+
+
+class ConfChooser:
+
+ # General Functions
+
+ def update_combo(self, combo, list):
+ combo.set_sensitive(False)
+ combo.get_model().clear()
+ current_index = 0
+ for (i, text) in enumerate(list):
+ combo.append_text(text)
+ if os.path.join(self.conf_dir, text) == os.path.realpath(self.conf_xml):
+ current_index = i
+ combo.set_active(current_index)
+ combo.set_sensitive(True)
+
+ def update_label(self):
+ desc = "Current conf: "
+ if not os.path.lexists(self.conf_xml):
+ desc += "does not exist"
+ else:
+ if os.path.islink(self.conf_xml):
+ if os.path.exists(self.conf_xml):
+ desc += "symlink to "
+ else:
+ desc += "broken symlink to "
+ real_conf_path = os.path.realpath(self.conf_xml)
+ desc += os.path.relpath(real_conf_path, self.conf_dir)
+ self.explain.set_text(desc)
+
+ # CallBack Functions
+
+
+ def find_conf_files(self):
+
+ conf_files = []
+
+ pattern = "conf[._-]*xml*"
+ backup_pattern = "conf[._-]*xml.20[0-9][0-9]-[01][0-9]-[0-3][0-9]_*"
+ excludes = ["%gconf.xml"]
+
+ for path, subdirs, files in os.walk(self.conf_dir):
+ for name in files:
+ if self.exclude_backups and fnmatch(name, backup_pattern):
+ continue
+ if fnmatch(name, pattern):
+ filepath = os.path.join(path, name)
+ entry = os.path.relpath(filepath, self.conf_dir)
+ if not os.path.islink(filepath) and entry not in excludes:
+ conf_files.append(entry)
+
+ conf_files.sort()
+ self.update_combo(self.conf_file_combo, conf_files)
+
+
+ def about(self):
+ gui_dialogs.about(paparazzi.home_dir)
+
+ def set_backups(self, widget):
+ self.exclude_backups = not widget.get_active()
+ self.find_conf_files()
+
+ def launch(self, widget):
+ os.system("./paparazzi &");
+ gtk.main_quit()
+
+ def backupconf(self, use_personal=False):
+ timestr = datetime.datetime.now().strftime("%Y-%m-%d_%H:%M")
+ if os.path.islink(self.conf_xml):
+ if self.verbose:
+ print("Symlink does not need backup");
+ else:
+ newname = "conf.xml." + timestr
+ backup_file = os.path.join(self.conf_dir, newname)
+ shutil.copyfile(self.conf_xml, backup_file)
+ print("Made a backup: " + newname)
+
+ if use_personal:
+ backup_name = self.conf_personal_name + "." + timestr
+ conf_personal_backup = os.path.join(self.conf_dir, backup_name)
+ if os.path.exists(self.conf_personal):
+ print("Backup conf.xml.personal to " + backup_name)
+ shutil.copyfile(self.conf_personal, conf_personal_backup)
+
+ def delete(self, widget):
+ filename = os.path.join(self.conf_dir, self.conf_file_combo.get_active_text())
+ # TODO: dialog: are you certain?
+ os.remove(filename)
+ self.update_label()
+ self.find_conf_files()
+
+
+ def accept(self, widget):
+ selected = self.conf_file_combo.get_active_text()
+ if selected == "conf.xml":
+ print("conf.xml is not a symlink, maybe you want to copy it to your personal file first?")
+ else:
+ self.backupconf()
+ link_source = self.conf_file_combo.get_active_text()
+ os.remove(self.conf_xml)
+ os.symlink(selected, self.conf_xml)
+ self.update_label()
+ self.find_conf_files()
+
+ def personal(self, widget):
+ if os.path.exists(self.conf_personal):
+ print("Your personal conf file already exists!")
+ else:
+ self.backupconf(True)
+ template_file = os.path.join(self.conf_dir, self.conf_file_combo.get_active_text())
+ shutil.copyfile(template_file, self.conf_personal)
+ os.remove(self.conf_xml)
+ os.symlink(self.conf_personal_name, self.conf_xml)
+ self.update_label()
+ self.find_conf_files()
+
+ # Constructor Functions
+
+ def destroy(self, widget, data=None):
+ gtk.main_quit()
+
+ def __init__(self):
+ self.window = gtk.Window(gtk.WINDOW_TOPLEVEL)
+ self.window.set_title("Paparazzi Configuration Chooser")
+
+ self.my_vbox = gtk.VBox()
+
+ # if PAPARAZZI_HOME not set, then assume the tree containing this
+ # file is a reasonable substitute
+ self.paparazzi_home = os.getenv("PAPARAZZI_HOME", os.path.dirname(os.path.abspath(__file__)))
+ self.conf_dir = os.path.join(self.paparazzi_home, "conf")
+ self.conf_xml = os.path.join(self.conf_dir, "conf.xml")
+ self.conf_personal_name = "conf_personal.xml"
+ self.conf_personal = os.path.join(self.conf_dir, self.conf_personal_name)
+
+ self.exclude_backups = True
+ self.verbose = False
+
+ # MenuBar
+ mb = gtk.MenuBar()
+
+ # File
+ filemenu = gtk.Menu()
+
+ # File Title
+ filem = gtk.MenuItem("File")
+ filem.set_submenu(filemenu)
+
+ exitm = gtk.MenuItem("Exit")
+ exitm.connect("activate", gtk.main_quit)
+ filemenu.append(exitm)
+
+ mb.append(filem)
+
+ # Help
+ helpmenu = gtk.Menu()
+
+ # Help Title
+ helpm = gtk.MenuItem("Help")
+ helpm.set_submenu(helpmenu)
+
+ aboutm = gtk.MenuItem("About")
+ aboutm.connect("activate", self.about)
+ helpmenu.append(aboutm)
+
+ mb.append(helpm)
+
+ self.my_vbox.pack_start(mb,False)
+
+ # Combo Bar
+
+ self.conf_label = gtk.Label("Conf:")
+
+ self.conf_file_combo = gtk.combo_box_new_text()
+ self.find_conf_files()
+# self.firmwares_combo.connect("changed", self.parse_list_of_airframes)
+ self.conf_file_combo.set_size_request(600,30)
+
+ self.confbar = gtk.HBox()
+ self.confbar.pack_start(self.conf_label)
+ self.confbar.pack_start(self.conf_file_combo)
+ self.my_vbox.pack_start(self.confbar, False)
+
+ ### show backups button
+ self.btnBackups = gtk.CheckButton("show backups")
+ self.btnBackups.connect("toggled", self.set_backups)
+ self.my_vbox.pack_start(self.btnBackups, False)
+
+ ##### Explain current config
+
+ self.explain = gtk.Label("");
+ self.update_label()
+
+ self.exbar = gtk.HBox()
+ self.exbar.pack_start(self.explain)
+
+ self.my_vbox.pack_start(self.exbar, False)
+
+ ##### Buttons
+ self.btnAccept = gtk.Button("Set Selected Conf As Active")
+ self.btnAccept.connect("clicked", self.accept)
+ self.btnAccept.set_tooltip_text("Set Conf as Active")
+
+ self.btnPersonal = gtk.Button("Create Personal Conf Based on Selected")
+ self.btnPersonal.connect("clicked", self.personal)
+ self.btnPersonal.set_tooltip_text("Create Personal Conf Based on Selected and Activate")
+
+ self.btnDelete = gtk.Button("Delete Selected")
+ self.btnDelete.connect("clicked", self.delete)
+ self.btnDelete.set_tooltip_text("Permanently Delete")
+
+ self.btnLaunch = gtk.Button("Launch Paparazzi")
+ self.btnLaunch.connect("clicked", self.launch)
+ self.btnLaunch.set_tooltip_text("Launch Paparazzi with current conf.xml")
+
+ self.btnExit = gtk.Button("Exit")
+ self.btnExit.connect("clicked", self.destroy)
+ self.btnExit.set_tooltip_text("Close application")
+
+
+ self.toolbar = gtk.HBox()
+ self.toolbar.pack_start(self.btnAccept)
+ self.toolbar.pack_start(self.btnPersonal)
+ self.toolbar.pack_start(self.btnDelete)
+ self.toolbar.pack_start(self.btnLaunch)
+ self.toolbar.pack_start(self.btnExit)
+
+ self.my_vbox.pack_start(self.toolbar, False)
+
+ ##### Bottom
+
+ self.window.add(self.my_vbox)
+ self.window.show_all()
+ self.window.set_position(gtk.WIN_POS_CENTER_ALWAYS)
+ self.window.connect("destroy", self.destroy)
+
+ def main(self):
+ gtk.main()
+
+if __name__ == "__main__":
+ import sys
+ if (len(sys.argv) > 1):
+ airframe_file = sys.argv[1]
+ gui = ConfChooser()
+ gui.main()
diff --git a/sw/airborne/arch/lpc21/mcu_periph/gpio_arch.h b/sw/airborne/arch/lpc21/mcu_periph/gpio_arch.h
index 8ccad960df..10ebdced39 100644
--- a/sw/airborne/arch/lpc21/mcu_periph/gpio_arch.h
+++ b/sw/airborne/arch/lpc21/mcu_periph/gpio_arch.h
@@ -34,7 +34,7 @@
/**
* Set a gpio output to high level.
*/
-static inline void gpio_output_high(uint32_t port, uint16_t pin) {
+static inline void gpio_set(uint32_t port, uint16_t pin) {
if (port == 0)
IO0SET = _BV(pin);
else if (port == 1)
@@ -44,7 +44,7 @@ static inline void gpio_output_high(uint32_t port, uint16_t pin) {
/**
* Clear a gpio output to low level.
*/
-static inline void gpio_output_low(uint32_t port, uint16_t pin) {
+static inline void gpio_clear(uint32_t port, uint16_t pin) {
if (port == 0)
IO0CLR = _BV(pin);
else if (port == 1)
diff --git a/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c b/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c
index 5838dcea14..c5379f91ea 100644
--- a/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c
+++ b/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c
@@ -336,8 +336,10 @@ bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t) {
idx = p->trans_insert_idx + 1;
if (idx >= I2C_TRANSACTION_QUEUE_LEN) idx = 0;
if (idx == p->trans_extract_idx) {
+ /* queue full */
+ p->errors->queue_full_cnt++;
t->status = I2CTransFailed;
- return FALSE; /* queue full */
+ return FALSE;
}
t->status = I2CTransPending;
diff --git a/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.c b/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.c
index c58302d997..18a2a6549d 100644
--- a/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.c
+++ b/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.c
@@ -47,7 +47,6 @@ uint8_t spi_slave_hs_tx_insert_idx, spi_slave_hs_tx_extract_idx;
uint8_t spi_slave_hs_tx_buffer[SPI_SLAVE_HS_TX_BUFFER_SIZE];
/* Prototypes */
-// void spi_init( void ); // -> declared in spi.h
static void SSP_ISR(void) __attribute__((naked));
/* SSPCR0 settings */
@@ -105,7 +104,7 @@ static void SSP_ISR(void) __attribute__((naked));
#endif
-void spi_init(void) {
+void spi_slave_hs_init(void) {
/* setup pins for SSP (SCK, MISO, MOSI) */
PINSEL1 |= SSP_PINSEL1_SCK | SSP_PINSEL1_MISO | SSP_PINSEL1_MOSI | SSP_PINSEL1_SSEL;
diff --git a/sw/airborne/arch/lpc21/mcu_periph/uart_arch.c b/sw/airborne/arch/lpc21/mcu_periph/uart_arch.c
index f209085ce2..5d594566c3 100644
--- a/sw/airborne/arch/lpc21/mcu_periph/uart_arch.c
+++ b/sw/airborne/arch/lpc21/mcu_periph/uart_arch.c
@@ -68,7 +68,7 @@ void uart_periph_set_baudrate(struct uart_periph* p, uint32_t baud) {
uart_enable_interrupts(p);
}
-void uart_periph_set_bits_stop_parity(struct uart_periph* __attribute__((unused)) p, uint8_t __attribute__((unused)) bits, uint8_t __attribute__((unused)) stop, uint8_t __attribute__((unused)) parity) {
+void uart_periph_set_bits_stop_parity(struct uart_periph* p __attribute__((unused)), uint8_t bits __attribute__((unused)), uint8_t stop __attribute__((unused)), uint8_t __attribute__((unused)) parity) {
// TBD
}
diff --git a/sw/airborne/arch/lpc21/mcu_periph/uart_arch.h b/sw/airborne/arch/lpc21/mcu_periph/uart_arch.h
index 874449f7b7..eedd044cd6 100644
--- a/sw/airborne/arch/lpc21/mcu_periph/uart_arch.h
+++ b/sw/airborne/arch/lpc21/mcu_periph/uart_arch.h
@@ -34,6 +34,17 @@
#include "LPC21xx.h"
#include BOARD_CONFIG
+#define B1200 1200
+#define B2400 2400
+#define B4800 4800
+#define B9600 9600
+#define B19200 19200
+#define B38400 38400
+#define B57600 57600
+#define B100000 100000
+#define B115200 115200
+#define B230400 230400
+
#define UART_8N1 (uint8_t)(ULCR_CHAR_8 + ULCR_PAR_NO + ULCR_STOP_1)
#define UART_7N1 (uint8_t)(ULCR_CHAR_7 + ULCR_PAR_NO + ULCR_STOP_1)
#define UART_8N2 (uint8_t)(ULCR_CHAR_8 + ULCR_PAR_NO + ULCR_STOP_2)
diff --git a/sw/airborne/arch/lpc21/subsystems/actuators/servos_ppm_hw.h b/sw/airborne/arch/lpc21/subsystems/actuators/servos_ppm_hw.h
index 6281a5dbd0..86fe9af00a 100644
--- a/sw/airborne/arch/lpc21/subsystems/actuators/servos_ppm_hw.h
+++ b/sw/airborne/arch/lpc21/subsystems/actuators/servos_ppm_hw.h
@@ -57,33 +57,33 @@ extern uint8_t ppm_pulse; /* 1=start of pulse, 0=end of pulse */
#define PPM_WIDTH SERVOS_TICS_OF_USEC(550)
#define ACTUATORS_IT TIR_MR1I
-#define ServosPPMMat_ISR() { \
- if (servos_PPM_idx == 0) { \
- servos_delay = SERVO_REFRESH_TICS; \
- } \
- if (servos_PPM_idx < _PPM_NB_CHANNELS ) { \
- if (ppm_pulse) { \
- T0MR1 += PPM_WIDTH; \
- servos_delay -= PPM_WIDTH; \
- } else { \
- T0MR1 += servos_values[servos_PPM_idx] - PPM_WIDTH; \
- servos_delay -= servos_values[servos_PPM_idx] - PPM_WIDTH; \
- servos_PPM_idx++; \
- } \
- } else { /* servos_PPM_idx=_PPM_NB_CHANNELS */ \
- if (ppm_pulse) { \
- T0MR1 += PPM_WIDTH; \
- servos_delay -= PPM_WIDTH; \
- } else { \
- servos_PPM_idx = 0; \
- T0MR1 += servos_delay - PPM_WIDTH; \
- } \
- } \
- if (!ppm_pulse) { \
- /* lower clock pin */ \
- T0EMR &= ~TEMR_EM1; \
- } \
- /* toggle ppm_pulse flag */ \
- ppm_pulse ^= 1; \
-}
+#define ServosPPMMat_ISR() { \
+ if (servos_PPM_idx == 0) { \
+ servos_delay = SERVO_REFRESH_TICS; \
+ } \
+ if (servos_PPM_idx < _PPM_NB_CHANNELS ) { \
+ if (ppm_pulse) { \
+ T0MR1 += PPM_WIDTH; \
+ servos_delay -= PPM_WIDTH; \
+ } else { \
+ T0MR1 += servos_values[servos_PPM_idx] - PPM_WIDTH; \
+ servos_delay -= servos_values[servos_PPM_idx] - PPM_WIDTH; \
+ servos_PPM_idx++; \
+ } \
+ } else { /* servos_PPM_idx=_PPM_NB_CHANNELS */ \
+ if (ppm_pulse) { \
+ T0MR1 += PPM_WIDTH; \
+ servos_delay -= PPM_WIDTH; \
+ } else { \
+ servos_PPM_idx = 0; \
+ T0MR1 += servos_delay - PPM_WIDTH; \
+ } \
+ } \
+ if (!ppm_pulse) { \
+ /* lower clock pin */ \
+ T0EMR &= ~TEMR_EM1; \
+ } \
+ /* toggle ppm_pulse flag */ \
+ ppm_pulse ^= 1; \
+ }
#endif /* SERVOS_PPM_HW_H */
diff --git a/sw/airborne/arch/omap/led_hw.h b/sw/airborne/arch/omap/led_hw.h
index 884d9581b3..6aecc2884d 100644
--- a/sw/airborne/arch/omap/led_hw.h
+++ b/sw/airborne/arch/omap/led_hw.h
@@ -28,10 +28,14 @@
#ifndef LED_HW_H_
#define LED_HW_H_
-#define LED_INIT(i) { }
-#define LED_ON(i) { }
-#define LED_OFF(i) { }
-#define LED_TOGGLE(i) { }
+#include
+
+extern uint32_t led_hw_values;
+
+#define LED_INIT(i) { led_hw_values &= ~(1<
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file arch/omap/mcu_periph/gpio_arch.h
+ *
+ * GPIO helper functions for linux/omap.
+ * @todo implement gpio_set|clear
+ */
+
+#ifndef GPIO_ARCH_H
+#define GPIO_ARCH_H
+
+#include "std.h"
+
+/**
+ * Set a gpio output to high level.
+ */
+extern void gpio_set(uint32_t port, uint16_t pin);
+
+/**
+ * Clear a gpio output to low level.
+ */
+extern void gpio_clear(uint32_t port, uint16_t pin);
+
+
+/**
+ * Read a gpio value.
+ */
+uint16_t gpio_get(uint32_t gpioport, uint16_t gpios);
+
+#endif /* GPIO_ARCH_H */
diff --git a/sw/airborne/arch/omap/mcu_periph/sys_time_arch.c b/sw/airborne/arch/omap/mcu_periph/sys_time_arch.c
index 2d4c78cd04..277e1a7893 100644
--- a/sw/airborne/arch/omap/mcu_periph/sys_time_arch.c
+++ b/sw/airborne/arch/omap/mcu_periph/sys_time_arch.c
@@ -29,6 +29,11 @@
#include
#include
+#ifdef SYS_TIME_LED
+#include "led.h"
+#endif
+
+
void sys_time_arch_init( void ) {
sys_time.cpu_ticks_per_sec = 1e6;
@@ -58,6 +63,10 @@ void sys_tick_handler( int signum ) {
if (sys_time.nb_sec_rem >= sys_time.cpu_ticks_per_sec) {
sys_time.nb_sec_rem -= sys_time.cpu_ticks_per_sec;
sys_time.nb_sec++;
+#ifdef SYS_TIME_LED
+ LED_TOGGLE(SYS_TIME_LED);
+#endif
+
}
for (unsigned int i=0; i
#include
-#include "fms/fms_serial_port.h"
+#include "serial_port.h"
+
+// #define TRACE(fmt,args...) fprintf(stderr, fmt, args)
+#define TRACE(fmt,args...)
-void uart_periph_set_baudrate(struct uart_periph* p, uint32_t baud) {
- struct FmsSerialPort* fmssp;
+void uart_periph_set_baudrate(struct uart_periph* periph, uint32_t baud) {
+ struct SerialPort* port;
// close serial port if already open
- if (p->reg_addr != NULL) {
- fmssp = (struct FmsSerialPort*)(p->reg_addr);
- serial_port_close(fmssp);
- serial_port_free(fmssp);
+ if (periph->reg_addr != NULL) {
+ port = (struct SerialPort*)(periph->reg_addr);
+ serial_port_close(port);
+ serial_port_free(port);
}
// open serial port
- fmssp = serial_port_new();
+ port = serial_port_new();
// use register address to store SerialPort structure pointer...
- p->reg_addr = (void*)fmssp;
+ periph->reg_addr = (void*)port;
//TODO: set device name in application and pass as argument
- printf("opening %s on uart0 at %d\n",p->dev,baud);
- serial_port_open_raw(fmssp,p->dev,baud);
+ // FIXME: paparazzi baud is 9600 for B9600 while open_raw needs 12 for B9600
+ printf("opening %s on uart0 at termios.h baud value=%d\n", periph->dev, baud);
+ int ret = serial_port_open_raw(port,periph->dev, baud);
+ if (ret != 0)
+ {
+ TRACE("Error opening %s code %d\n",periph->dev,ret);
+ }
}
-void uart_transmit(struct uart_periph* p, uint8_t data ) {
- uint16_t temp = (p->tx_insert_idx + 1) % UART_TX_BUFFER_SIZE;
+void uart_transmit(struct uart_periph* periph, uint8_t data) {
+ uint16_t temp = (periph->tx_insert_idx + 1) % UART_TX_BUFFER_SIZE;
- if (temp == p->tx_extract_idx)
+ if (temp == periph->tx_extract_idx)
return; // no room
// check if in process of sending data
- if (p->tx_running) { // yes, add to queue
- p->tx_buf[p->tx_insert_idx] = data;
- p->tx_insert_idx = temp;
+ if (periph->tx_running) { // yes, add to queue
+ periph->tx_buf[periph->tx_insert_idx] = data;
+ periph->tx_insert_idx = temp;
}
else { // no, set running flag and write to output register
- p->tx_running = TRUE;
- struct FmsSerialPort* fmssp = (struct FmsSerialPort*)(p->reg_addr);
- write((int)(fmssp->fd),&data,1);
- //printf("w %x\n",data);
+ periph->tx_running = TRUE;
+ struct SerialPort* port = (struct SerialPort*)(periph->reg_addr);
+ int ret = write((int)(port->fd), &data, 1);
+ if (ret < 1)
+ {
+ TRACE("w %x [%d]\n",data,ret);
+ }
}
}
-static inline void uart_handler(struct uart_periph* p) {
+#include
+
+static inline void uart_handler(struct uart_periph* periph) {
unsigned char c='D';
- if (p->reg_addr == NULL) return; // device not initialized ?
+ if (periph->reg_addr == NULL) return; // device not initialized ?
- struct FmsSerialPort* fmssp = (struct FmsSerialPort*)(p->reg_addr);
- int fd = fmssp->fd;
+ struct SerialPort* port = (struct SerialPort*)(periph->reg_addr);
+ int fd = port->fd;
// check if more data to send
- if (p->tx_insert_idx != p->tx_extract_idx) {
- write(fd,&(p->tx_buf[p->tx_extract_idx]),1);
- //printf("w %x\n",p->tx_buf[p->tx_extract_idx]);
- p->tx_extract_idx++;
- p->tx_extract_idx %= UART_TX_BUFFER_SIZE;
+ if (periph->tx_insert_idx != periph->tx_extract_idx) {
+ int ret = write(fd, &(periph->tx_buf[periph->tx_extract_idx]), 1);
+ if (ret < 1)
+ {
+ TRACE("w %x [%d: %s]\n", periph->tx_buf[periph->tx_extract_idx], ret, strerror(errno));
+ }
+ periph->tx_extract_idx++;
+ periph->tx_extract_idx %= UART_TX_BUFFER_SIZE;
}
else {
- p->tx_running = FALSE; // clear running flag
+ periph->tx_running = FALSE; // clear running flag
}
if(read(fd,&c,1) > 0){
//printf("r %x %c\n",c,c);
- uint16_t temp = (p->rx_insert_idx + 1) % UART_RX_BUFFER_SIZE;
- p->rx_buf[p->rx_insert_idx] = c;
+ uint16_t temp = (periph->rx_insert_idx + 1) % UART_RX_BUFFER_SIZE;
+ periph->rx_buf[periph->rx_insert_idx] = c;
// check for more room in queue
- if (temp != p->rx_extract_idx)
- p->rx_insert_idx = temp; // update insert index
+ if (temp != periph->rx_extract_idx)
+ periph->rx_insert_idx = temp; // update insert index
}
}
diff --git a/sw/airborne/arch/omap/mcu_periph/uart_arch.h b/sw/airborne/arch/omap/mcu_periph/uart_arch.h
index d5a6fd4301..24c341234f 100644
--- a/sw/airborne/arch/omap/mcu_periph/uart_arch.h
+++ b/sw/airborne/arch/omap/mcu_periph/uart_arch.h
@@ -27,7 +27,9 @@
#define UART_ARCH_H
#include "mcu_periph/uart.h"
-#include "std.h"
+
+// for definition of baud rates
+#include
#define UART1_irq_handler usart1_irq_handler
#define UART2_irq_handler usart2_irq_handler
diff --git a/sw/airborne/fms/fms_serial_port.c b/sw/airborne/arch/omap/serial_port.c
similarity index 83%
rename from sw/airborne/fms/fms_serial_port.c
rename to sw/airborne/arch/omap/serial_port.c
index d42bedfd16..13441f77b0 100644
--- a/sw/airborne/fms/fms_serial_port.c
+++ b/sw/airborne/arch/omap/serial_port.c
@@ -1,4 +1,4 @@
-#include "fms_serial_port.h"
+#include "serial_port.h"
#include
#include
@@ -13,37 +13,37 @@
#define TRACE(type,fmt,args...)
#define TRACE_ERROR 1
-struct FmsSerialPort* serial_port_new(void) {
- struct FmsSerialPort* me = malloc(sizeof(struct FmsSerialPort));
+struct SerialPort* serial_port_new(void) {
+ struct SerialPort* me = malloc(sizeof(struct SerialPort));
return me;
}
-void serial_port_free(struct FmsSerialPort* me) {
+void serial_port_free(struct SerialPort* me) {
free(me);
}
-void serial_port_flush(struct FmsSerialPort* me) {
+void serial_port_flush(struct SerialPort* me) {
/*
* flush any input that might be on the port so we start fresh.
*/
if (tcflush(me->fd, TCIFLUSH)) {
- TRACE(TRACE_ERROR,"%s, set term attr failed: %s (%d)\n", device, strerror(errno), errno);
+ TRACE(TRACE_ERROR,"%s, set term attr failed: %s (%d)\n", "", strerror(errno), errno);
fprintf(stderr, "flush (%d) failed: %s (%d)\n", me->fd, strerror(errno), errno);
}
}
-void serial_port_flush_output(struct FmsSerialPort* me) {
+void serial_port_flush_output(struct SerialPort* me) {
/*
* flush any input that might be on the port so we start fresh.
*/
if (tcflush(me->fd, TCOFLUSH)) {
- TRACE(TRACE_ERROR,"%s, set term attr failed: %s (%d)\n", device, strerror(errno), errno);
+ TRACE(TRACE_ERROR,"%s, set term attr failed: %s (%d)\n", "", strerror(errno), errno);
fprintf(stderr, "flush (%d) failed: %s (%d)\n", me->fd, strerror(errno), errno);
}
}
-int serial_port_open_raw(struct FmsSerialPort* me, const char* device, speed_t speed) {
+int serial_port_open_raw(struct SerialPort* me, const char* device, speed_t speed) {
if ((me->fd = open(device, O_RDWR | O_NONBLOCK | O_NOCTTY)) < 0) {
TRACE(TRACE_ERROR,"%s, open failed: %s (%d)\n", device, strerror(errno), errno);
return -1;
@@ -78,7 +78,7 @@ int serial_port_open_raw(struct FmsSerialPort* me, const char* device, speed_t
return 0;
}
-int serial_port_open(struct FmsSerialPort* me, const char* device,
+int serial_port_open(struct SerialPort* me, const char* device,
void(*term_conf_callback)(struct termios*, speed_t*)) {
speed_t speed;
@@ -108,7 +108,7 @@ int serial_port_open(struct FmsSerialPort* me, const char* device,
}
-void serial_port_close(struct FmsSerialPort* me) {
+void serial_port_close(struct SerialPort* me) {
/* if null pointer or file descriptor indicates error just bail */
if (!me || me->fd < 0)
diff --git a/sw/airborne/fms/fms_serial_port.h b/sw/airborne/arch/omap/serial_port.h
similarity index 66%
rename from sw/airborne/fms/fms_serial_port.h
rename to sw/airborne/arch/omap/serial_port.h
index 44a120c73c..f259114975 100644
--- a/sw/airborne/fms/fms_serial_port.h
+++ b/sw/airborne/arch/omap/serial_port.h
@@ -20,24 +20,24 @@
*
*/
-#ifndef FMS_SERIAL_PORT_H
-#define FMS_SERIAL_PORT_H
+#ifndef SERIAL_PORT_H
+#define SERIAL_PORT_H
#include
-struct FmsSerialPort {
+struct SerialPort {
int fd; /* serial device fd */
struct termios orig_termios; /* saved tty state structure */
struct termios cur_termios; /* tty state structure */
};
-extern struct FmsSerialPort* serial_port_new(void);
-extern void serial_port_free(struct FmsSerialPort* me);
-extern void serial_port_flush(struct FmsSerialPort* me);
-extern void serial_port_flush_output(struct FmsSerialPort* me);
-extern int serial_port_open_raw(struct FmsSerialPort* me, const char* device, speed_t speed);
-extern int serial_port_open(struct FmsSerialPort* me, const char* device,
+extern struct SerialPort* serial_port_new(void);
+extern void serial_port_free(struct SerialPort* me);
+extern void serial_port_flush(struct SerialPort* me);
+extern void serial_port_flush_output(struct SerialPort* me);
+extern int serial_port_open_raw(struct SerialPort* me, const char* device, speed_t speed);
+extern int serial_port_open(struct SerialPort* me, const char* device,
void(*term_conf_callback)(struct termios*, speed_t*));
-extern void serial_port_close(struct FmsSerialPort* me);
+extern void serial_port_close(struct SerialPort* me);
-#endif /* FMS_SERIAL_PORT_H */
+#endif /* SERIAL_PORT_H */
diff --git a/sw/airborne/arch/stm32/lisa-s.ld b/sw/airborne/arch/stm32/lisa-s.ld
new file mode 100644
index 0000000000..a919196043
--- /dev/null
+++ b/sw/airborne/arch/stm32/lisa-s.ld
@@ -0,0 +1,36 @@
+/*
+ * Hey Emacs, this is a -*- makefile -*-
+ *
+ * Copyright (C) 2012 Piotr Esden-Tempski
+ *
+ * This file is part of Paparazzi.
+ *
+ * Paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * Paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/* Linker script for Lisa-S (STM32F103REY6, 512K flash, 64K RAM). */
+
+/* Define memory regions. */
+MEMORY
+{
+ ram (rwx) : ORIGIN = 0x20000000, LENGTH = 64K
+ /* Leaving 2k of space at the end of rom for stored settings */
+ rom (rx) : ORIGIN = 0x08000000, LENGTH = 512K
+}
+
+/* Include the common ld script. */
+INCLUDE libopencm3_stm32f1.ld
+
diff --git a/sw/airborne/arch/stm32/mcu_periph/gpio_arch.h b/sw/airborne/arch/stm32/mcu_periph/gpio_arch.h
index 9d62af5e76..b9c27eebfe 100644
--- a/sw/airborne/arch/stm32/mcu_periph/gpio_arch.h
+++ b/sw/airborne/arch/stm32/mcu_periph/gpio_arch.h
@@ -24,6 +24,8 @@
* @ingroup stm32_arch
*
* GPIO helper functions for STM32F1 and STM32F4.
+ *
+ * The gpio_set and gpio_clear functions are already available from libopencm3.
*/
#ifndef GPIO_ARCH_H
@@ -31,20 +33,6 @@
#include
-/**
- * Set a gpio output to high level.
- */
-static inline void gpio_output_high(uint32_t port, uint16_t pin) {
- gpio_set(port, pin);
-}
-
-/**
- * Clear a gpio output to low level.
- */
-static inline void gpio_output_low(uint32_t port, uint16_t pin) {
- gpio_clear(port, pin);
-}
-
/**
* Setup a gpio for input or output with alternate function.
*/
diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c
index 902d915759..38e737db23 100644
--- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c
+++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c
@@ -1090,17 +1090,17 @@ void i2c3_hw_init(void) {
/* Enable I2C3 clock */
rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_I2C3EN);
/* Enable GPIO clock */
- gpio_enable_clock(I2C3_GPIO_SCL_PORT);
- gpio_mode_setup(I2C3_GPIO_SCL_PORT, GPIO_MODE_AF, GPIO_PUPD_NONE, I2C3_GPIO_SCL);
- gpio_set_output_options(I2C3_GPIO_SCL_PORT, GPIO_OTYPE_OD, GPIO_OSPEED_25MHZ,
+ gpio_enable_clock(I2C3_GPIO_PORT_SCL);
+ gpio_mode_setup(I2C3_GPIO_PORT_SCL, GPIO_MODE_AF, GPIO_PUPD_NONE, I2C3_GPIO_SCL);
+ gpio_set_output_options(I2C3_GPIO_PORT_SCL, GPIO_OTYPE_OD, GPIO_OSPEED_25MHZ,
I2C3_GPIO_SCL);
- gpio_set_af(I2C3_GPIO_SCL_PORT, GPIO_AF4, I2C3_GPIO_SCL);
+ gpio_set_af(I2C3_GPIO_PORT_SCL, GPIO_AF4, I2C3_GPIO_SCL);
- gpio_enable_clock(I2C3_GPIO_SDA_PORT);
- gpio_mode_setup(I2C3_GPIO_SDA_PORT, GPIO_MODE_AF, GPIO_PUPD_NONE, I2C3_GPIO_SDA);
- gpio_set_output_options(I2C3_GPIO_SDA_PORT, GPIO_OTYPE_OD, GPIO_OSPEED_25MHZ,
+ gpio_enable_clock(I2C3_GPIO_PORT_SDA);
+ gpio_mode_setup(I2C3_GPIO_PORT_SDA, GPIO_MODE_AF, GPIO_PUPD_NONE, I2C3_GPIO_SDA);
+ gpio_set_output_options(I2C3_GPIO_PORT_SDA, GPIO_OTYPE_OD, GPIO_OSPEED_25MHZ,
I2C3_GPIO_SDA);
- gpio_set_af(I2C3_GPIO_SDA_PORT, GPIO_AF4, I2C3_GPIO_SDA);
+ gpio_set_af(I2C3_GPIO_PORT_SDA, GPIO_AF4, I2C3_GPIO_SDA);
i2c_reset(I2C3);
@@ -1316,8 +1316,12 @@ bool_t i2c_submit(struct i2c_periph* periph, struct i2c_transaction* t) {
uint8_t temp;
temp = periph->trans_insert_idx + 1;
if (temp >= I2C_TRANSACTION_QUEUE_LEN) temp = 0;
- if (temp == periph->trans_extract_idx)
- return FALSE; // queue full
+ if (temp == periph->trans_extract_idx) {
+ // queue full
+ periph->errors->queue_full_cnt++;
+ t->status = I2CTransFailed;
+ return FALSE;
+ }
t->status = I2CTransPending;
diff --git a/sw/airborne/arch/stm32/mcu_periph/spi_arch.c b/sw/airborne/arch/stm32/mcu_periph/spi_arch.c
index bb5d82876c..f2b5cc79f4 100644
--- a/sw/airborne/arch/stm32/mcu_periph/spi_arch.c
+++ b/sw/airborne/arch/stm32/mcu_periph/spi_arch.c
@@ -1201,12 +1201,217 @@ void process_tx_dma_interrupt(struct spi_periph *periph) {
/*
*
* SPI Slave code
- *
- * FIXME implement it
+ * Currently only for F1, SPI1
*
*/
#ifdef SPI_SLAVE
-#warning SPI_SLAVE mode currently not implemented for STM32.
+static void process_slave_rx_dma_interrupt(struct spi_periph* periph);
+
+
+// SPI arch slave init
+#if USE_SPI1_SLAVE
+#warning "SPI1 slave: Untested code!"
+
+#ifndef STM32F1
+#error "SPI1 slave on STM32 only implemented for STM32F1"
+#endif
+
+#if USE_SPI1
+#error "Using SPI1 as a slave and master at the same time is not possible."
+#endif
+
+static struct spi_periph_dma spi1_dma;
+
+void spi1_slave_arch_init(void) {
+ // set dma options
+ spi1_dma.spidr = (uint32_t)&SPI1_DR;
+ spi1_dma.dma = DMA1;
+ spi1_dma.rx_chan = DMA_CHANNEL2;
+ spi1_dma.tx_chan = DMA_CHANNEL3;
+ spi1_dma.rx_nvic_irq = NVIC_DMA1_CHANNEL2_IRQ;
+ spi1_dma.tx_nvic_irq = NVIC_DMA1_CHANNEL3_IRQ;
+ spi1_dma.tx_dummy_buf = 0;
+ spi1_dma.tx_extra_dummy_dma = FALSE;
+ spi1_dma.rx_dummy_buf = 0;
+ spi1_dma.rx_extra_dummy_dma = FALSE;
+
+ // set the default configuration
+ set_default_comm_config(&spi1_dma.comm);
+ spi1_dma.comm_sig = get_comm_signature(&spi1_dma.comm);
+
+ // set init struct, indices and status
+ spi1.reg_addr = (void *)SPI1;
+ spi1.init_struct = &spi1_dma;
+ spi1.trans_insert_idx = 0;
+ spi1.trans_extract_idx = 0;
+ spi1.status = SPIIdle;
+
+ // Enable SPI1 Periph and gpio clocks
+ rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_SPI1EN);
+
+ // Configure GPIOs: SCK, MISO and MOSI
+ // TODO configure lisa board files to use gpio_setup_pin_af function
+ gpio_set_mode(GPIO_BANK_SPI1_SCK, GPIO_MODE_INPUT,
+ GPIO_CNF_INPUT_FLOAT,
+ GPIO_SPI1_SCK | GPIO_SPI1_MOSI);
+
+ gpio_set_mode(GPIO_BANK_SPI1_MISO, GPIO_MODE_OUTPUT_50_MHZ,
+ GPIO_CNF_OUTPUT_ALTFN_PUSHPULL,
+ GPIO_SPI1_MISO);
+
+ gpio_set_mode(GPIO_BANK_SPI1_NSS, GPIO_MODE_INPUT,
+ GPIO_CNF_INPUT_FLOAT,
+ GPIO_SPI1_NSS);
+
+ // reset SPI
+ spi_reset(SPI1);
+
+ // Disable SPI peripheral
+ spi_disable(SPI1);
+
+ // Force SPI mode over I2S.
+ SPI1_I2SCFGR = 0;
+
+ // configure master SPI.
+ spi_init_master(SPI1, spi1_dma.comm.br, spi1_dma.comm.cpol, spi1_dma.comm.cpha,
+ spi1_dma.comm.dff, spi1_dma.comm.lsbfirst);
+
+ spi_disable_software_slave_management(SPI1);
+
+ spi_set_slave_mode(SPI1);
+
+ // Enable SPI_1 DMA clock
+#ifdef STM32F1
+ rcc_peripheral_enable_clock(&RCC_AHBENR, RCC_AHBENR_DMA1EN);
+#elif defined STM32F4
+ rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_DMA2EN);
+#endif
+
+ // Enable SPI1 periph.
+ spi_enable(SPI1);
+
+ spi_arch_int_enable(&spi1);
+}
+
+/// receive transferred over DMA
+void dma1_channel2_isr(void)
+{
+ if ((DMA1_ISR & DMA_ISR_TCIF2) != 0) {
+ // clear int pending bit
+ DMA1_IFCR |= DMA_IFCR_CTCIF2;
+ }
+ process_slave_rx_dma_interrupt(&spi1);
+}
+
+
+#endif /* USE_SPI1_SLAVE */
+
+
+static void spi_slave_set_config(struct spi_periph* periph, struct spi_transaction* trans)
+{
+ struct spi_periph_dma *dma;
+
+ dma = periph->init_struct;
+
+ set_comm_from_transaction(&(dma->comm), trans);
+
+ /* remember the new conf signature */
+ //dma->comm_sig = sig;
+
+ /* apply the new configuration */
+ spi_disable((uint32_t)periph->reg_addr);
+ spi_init_master((uint32_t)periph->reg_addr, dma->comm.br, dma->comm.cpol,
+ dma->comm.cpha, dma->comm.dff, dma->comm.lsbfirst);
+ spi_disable_software_slave_management((uint32_t)periph->reg_addr);
+ spi_set_slave_mode((uint32_t)periph->reg_addr);
+ spi_enable((uint32_t)periph->reg_addr);
+}
+
+
+
+//static void spi_start_slave_dma_transaction(struct spi_periph* periph, struct spi_transaction* trans)
+bool_t spi_slave_register(struct spi_periph* periph, struct spi_transaction* trans)
+{
+ spi_slave_set_config(periph, trans);
+
+ struct spi_periph_dma *dma;
+ uint8_t sig = 0x00;
+
+ /* Store local copy to notify of the results */
+ trans->status = SPITransRunning;
+ periph->status = SPIRunning;
+
+ periph->trans_insert_idx = 0;
+ periph->trans[periph->trans_insert_idx] = trans;
+
+ dma = periph->init_struct;
+
+ /*
+ * Receive DMA channel configuration ----------------------------------------
+ */
+ spi_configure_dma(dma->dma, dma->rx_chan, (uint32_t)dma->spidr,
+ (uint32_t)trans->input_buf, trans->input_length, trans->dss, TRUE);
+
+ dma_set_read_from_peripheral(dma->dma, dma->rx_chan);
+ dma_set_priority(dma->dma, dma->rx_chan, DMA_CCR_PL_VERY_HIGH);
+
+
+ /*
+ * Transmit DMA channel configuration ---------------------------------------
+ */
+ spi_configure_dma(dma->dma, dma->tx_chan, (uint32_t)dma->spidr,
+ (uint32_t)trans->output_buf, trans->output_length, trans->dss, TRUE);
+
+ dma_set_read_from_memory(dma->dma, dma->tx_chan);
+ dma_set_priority(dma->dma, dma->tx_chan, DMA_CCR_PL_MEDIUM);
+
+ /* Enable DMA transfer complete interrupts. */
+ dma_enable_transfer_complete_interrupt(dma->dma, dma->rx_chan);
+
+ /* Enable DMA channels */
+ dma_enable_channel(dma->dma, dma->rx_chan);
+ dma_enable_channel(dma->dma, dma->tx_chan);
+
+ /* Enable SPI transfers via DMA */
+ spi_enable_rx_dma((uint32_t)periph->reg_addr);
+ spi_enable_tx_dma((uint32_t)periph->reg_addr);
+
+ return TRUE;
+}
+
+void process_slave_rx_dma_interrupt(struct spi_periph *periph) {
+ struct spi_periph_dma *dma = periph->init_struct;
+ struct spi_transaction *trans = periph->trans[periph->trans_extract_idx];
+
+ /* Disable DMA Channel */
+ dma_disable_transfer_complete_interrupt(dma->dma, dma->rx_chan);
+
+ /* Disable SPI Rx request */
+ spi_disable_rx_dma((uint32_t)periph->reg_addr);
+
+ /* Disable DMA rx channel */
+ dma_disable_channel(dma->dma, dma->rx_chan);
+
+ /* Run the callback */
+ trans->status = SPITransSuccess;
+ //return;
+ if (trans->after_cb != 0) {
+ trans->after_cb(trans);
+ }
+
+ /*dma_enable_transfer_complete_interrupt(dma->dma, dma->rx_chan);
+ dma_enable_channel(dma->dma, dma->rx_chan);
+ spi_enable_rx_dma((uint32_t)periph->reg_addr);*/
+}
+
+
+// Slave Select / NSS pin GPIO config
+
+
+// DMA config?
+
+
+// SPI transaction handling
#endif /* SPI_SLAVE */
diff --git a/sw/airborne/arch/stm32/mcu_periph/uart_arch.h b/sw/airborne/arch/stm32/mcu_periph/uart_arch.h
index a375fb83ee..0b8346ad87 100644
--- a/sw/airborne/arch/stm32/mcu_periph/uart_arch.h
+++ b/sw/airborne/arch/stm32/mcu_periph/uart_arch.h
@@ -29,6 +29,16 @@
#ifndef STM32_UART_ARCH_H
#define STM32_UART_ARCH_H
-#include "std.h"
+#define B1200 1200
+#define B2400 2400
+#define B4800 4800
+#define B9600 9600
+#define B19200 19200
+#define B38400 38400
+#define B57600 57600
+#define B100000 100000
+#define B115200 115200
+#define B230400 230400
+#define B921600 921600
#endif /* STM32_UART_ARCH_H */
diff --git a/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c b/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c
index 6a4ba6bf7e..70e309b299 100644
--- a/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c
+++ b/sw/airborne/arch/stm32/subsystems/actuators/actuators_pwm_arch.c
@@ -23,6 +23,8 @@
* STM32 PWM servos handling.
*/
+//VALID TIMERS ARE TIM1,2,3,4,5,8,9,12
+
#include "subsystems/actuators/actuators_pwm_arch.h"
#include "subsystems/actuators/actuators_pwm.h"
@@ -30,7 +32,6 @@
#include
#include
-int32_t actuators_pwm_values[ACTUATORS_PWM_NB];
#if defined(STM32F1)
//#define PCLK 72000000
@@ -41,6 +42,27 @@ int32_t actuators_pwm_values[ACTUATORS_PWM_NB];
#endif
#define ONE_MHZ_CLK 1000000
+
+#ifdef STM32F1
+/**
+ * HCLK = 72MHz, Timer clock also 72MHz since
+ * TIM1_CLK = APB2 = 72MHz
+ * TIM2_CLK = 2 * APB1 = 2 * 32MHz
+ */
+#define TIMER_APB1_CLK AHB_CLK
+#define TIMER_APB2_CLK AHB_CLK
+#endif
+
+#ifdef STM32F4
+/* Since APB prescaler != 1 :
+ * Timer clock frequency (before prescaling) is twice the frequency
+ * of the APB domain to which the timer is connected.
+ */
+#define TIMER_APB1_CLK (rcc_ppre1_frequency * 2)
+#define TIMER_APB2_CLK (rcc_ppre2_frequency * 2)
+#endif
+
+
/** Default servo update rate in Hz */
#ifndef SERVO_HZ
#define SERVO_HZ 40
@@ -62,19 +84,37 @@ int32_t actuators_pwm_values[ACTUATORS_PWM_NB];
#ifndef TIM5_SERVO_HZ
#define TIM5_SERVO_HZ SERVO_HZ
#endif
+#ifndef TIM9_SERVO_HZ
+#define TIM9_SERVO_HZ SERVO_HZ
+#endif
+#ifndef TIM12_SERVO_HZ
+#define TIM12_SERVO_HZ SERVO_HZ
+#endif
+
+
+/** @todo: these should go into libopencm3 */
+#define TIM9 TIM9_BASE
+#define TIM12 TIM12_BASE
+
+int32_t actuators_pwm_values[ACTUATORS_PWM_NB];
+
/** Set PWM channel configuration
*/
static inline void actuators_pwm_arch_channel_init(uint32_t timer_peripheral,
- enum tim_oc_id oc_id) {
+ enum tim_oc_id oc_id) {
timer_disable_oc_output(timer_peripheral, oc_id);
- timer_disable_oc_clear(timer_peripheral, oc_id);
+ //There is no such register in TIM9 and 12.
+ if (timer_peripheral != TIM9 && timer_peripheral != TIM12)
+ timer_disable_oc_clear(timer_peripheral, oc_id);
timer_enable_oc_preload(timer_peripheral, oc_id);
timer_set_oc_slow_mode(timer_peripheral, oc_id);
timer_set_oc_mode(timer_peripheral, oc_id, TIM_OCM_PWM1);
timer_set_oc_polarity_high(timer_peripheral, oc_id);
timer_enable_oc_output(timer_peripheral, oc_id);
+ // Used for TIM1 and TIM8, the function does nothing if other timer is specified.
+ timer_enable_break_main_output(timer_peripheral);
}
/** Set GPIO configuration
@@ -103,9 +143,20 @@ static inline void set_servo_timer(uint32_t timer, uint32_t period, uint8_t chan
* - Alignement edge.
* - Direction up.
*/
- timer_set_mode(timer, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP);
+ if ((timer == TIM9) || (timer == TIM12))
+ //There are no EDGE and DIR settings in TIM9 and TIM12
+ timer_set_mode(timer, TIM_CR1_CKD_CK_INT, 0, 0);
+ else
+ timer_set_mode(timer, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP);
- timer_set_prescaler(timer, (PCLK / ONE_MHZ_CLK) - 1); // 1uS
+
+ // TIM1, 8 and 9 use APB2 clock, all others APB1
+ if (timer != TIM1 && timer != TIM8 && timer != TIM9) {
+ timer_set_prescaler(timer, (TIMER_APB1_CLK / ONE_MHZ_CLK) - 1); // 1uS
+ } else {
+ // TIM9, 1 and 8 use APB2 clock
+ timer_set_prescaler(timer, (TIMER_APB2_CLK / ONE_MHZ_CLK) - 1);
+ }
timer_disable_preload(timer);
@@ -148,6 +199,7 @@ static inline void set_servo_timer(uint32_t timer, uint32_t period, uint8_t chan
/* Counter enable. */
timer_enable_counter(timer);
+
}
/** PWM arch init called by generic pwm driver
@@ -158,6 +210,7 @@ void actuators_pwm_arch_init(void) {
* Configure timer peripheral clocks
*-----------------------------------*/
#if PWM_USE_TIM1
+ // APB2 runs on double the frequency of APB1.
rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_TIM1EN);
#endif
#if PWM_USE_TIM2
@@ -172,6 +225,17 @@ void actuators_pwm_arch_init(void) {
#if PWM_USE_TIM5
rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_TIM5EN);
#endif
+#if PWM_USE_TIM8
+ // APB2 runs on double the frequency of APB1.
+ rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_TIM8EN);
+#endif
+#if PWM_USE_TIM9
+ // APB2 runs on double the frequency of APB1.
+ rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_TIM9EN);
+#endif
+#if PWM_USE_TIM12
+ rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_TIM12EN);
+#endif
/*----------------
* Configure GPIO
@@ -211,6 +275,12 @@ void actuators_pwm_arch_init(void) {
#ifdef PWM_SERVO_9
set_servo_gpio(PWM_SERVO_9_GPIO, PWM_SERVO_9_PIN, PWM_SERVO_9_AF, PWM_SERVO_9_RCC_IOP);
#endif
+#ifdef PWM_SERVO_10
+ set_servo_gpio(PWM_SERVO_10_GPIO, PWM_SERVO_10_PIN, PWM_SERVO_10_AF, PWM_SERVO_10_RCC_IOP);
+#endif
+#ifdef PWM_SERVO_11
+ set_servo_gpio(PWM_SERVO_11_GPIO, PWM_SERVO_11_PIN, PWM_SERVO_11_AF, PWM_SERVO_11_RCC_IOP);
+#endif
#if PWM_USE_TIM1
@@ -233,6 +303,18 @@ void actuators_pwm_arch_init(void) {
set_servo_timer(TIM5, TIM5_SERVO_HZ, PWM_TIM5_CHAN_MASK);
#endif
+#if PWM_USE_TIM8
+ set_servo_timer(TIM8, TIM8_SERVO_HZ, PWM_TIM8_CHAN_MASK);
+#endif
+
+#if PWM_USE_TIM9
+ set_servo_timer(TIM9, TIM9_SERVO_HZ, PWM_TIM9_CHAN_MASK);
+#endif
+
+#if PWM_USE_TIM12
+ set_servo_timer(TIM12, TIM12_SERVO_HZ, PWM_TIM12_CHAN_MASK);
+#endif
+
}
/** Set pulse widths from actuator values, assumed to be in us
@@ -268,5 +350,11 @@ void actuators_pwm_commit(void) {
#ifdef PWM_SERVO_9
timer_set_oc_value(PWM_SERVO_9_TIMER, PWM_SERVO_9_OC, actuators_pwm_values[PWM_SERVO_9]);
#endif
+#ifdef PWM_SERVO_10
+ timer_set_oc_value(PWM_SERVO_10_TIMER, PWM_SERVO_10_OC, actuators_pwm_values[PWM_SERVO_10]);
+#endif
+#ifdef PWM_SERVO_11
+ timer_set_oc_value(PWM_SERVO_11_TIMER, PWM_SERVO_11_OC, actuators_pwm_values[PWM_SERVO_11]);
+#endif
}
diff --git a/sw/airborne/arch/stm32/subsystems/imu/imu_krooz_sd_arch.c b/sw/airborne/arch/stm32/subsystems/imu/imu_krooz_sd_arch.c
index 64ef448548..a13032bf7c 100644
--- a/sw/airborne/arch/stm32/subsystems/imu/imu_krooz_sd_arch.c
+++ b/sw/airborne/arch/stm32/subsystems/imu/imu_krooz_sd_arch.c
@@ -28,10 +28,10 @@ void exti9_5_isr(void) {
/* clear EXTI */
if(EXTI_PR & EXTI6) {
exti_reset_request(EXTI6);
- hmc58xx_read(&imu_krooz.hmc);
+ imu_krooz.hmc_eoc = TRUE;
}
if(EXTI_PR & EXTI5) {
exti_reset_request(EXTI5);
- mpu60x0_i2c_read(&imu_krooz.mpu);
+ imu_krooz.mpu_eoc = TRUE;
}
}
diff --git a/sw/airborne/boards/apogee_1.0.h b/sw/airborne/boards/apogee_1.0.h
index 78ff267397..343c514eca 100644
--- a/sw/airborne/boards/apogee_1.0.h
+++ b/sw/airborne/boards/apogee_1.0.h
@@ -1,5 +1,5 @@
-#ifndef CONFIG_APOGEE_0_99_H
-#define CONFIG_APOGEE_0_99_H
+#ifndef CONFIG_APOGEE_1_00_H
+#define CONFIG_APOGEE_1_00_H
#define BOARD_APOGEE
@@ -308,7 +308,7 @@
#endif
#if USE_PWM3
-#define PWM_SERVO_3_IDX 3
+#define PWM_SERVO_3 3
#define PWM_SERVO_3_TIMER TIM3
#define PWM_SERVO_3_RCC_IOP RCC_AHB1ENR_IOPBEN
#define PWM_SERVO_3_GPIO GPIOB
@@ -396,4 +396,4 @@
#define SPEKTRUM_UART2_ISR usart2_isr
#define SPEKTRUM_UART2_DEV USART2
-#endif /* CONFIG_APOGEE_0_99_H */
+#endif /* CONFIG_APOGEE_1_00_H */
diff --git a/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c b/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c
index 353c5a5050..b7ea3f9cae 100644
--- a/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c
+++ b/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c
@@ -30,7 +30,10 @@
#include "subsystems/actuators.h"
#include "actuators_ardrone2_raw.h"
-#include "gpio_ardrone.h"
+#include "mcu_periph/gpio.h"
+#include "led_hw.h"
+#include "mcu_periph/sys_time.h"
+#include "navdata.h" // for full_write
#include /* Standard input/output definitions */
#include /* String function definitions */
@@ -50,24 +53,49 @@
* 190 2.5
* 130 3.0
*/
-int mot_fd; /**< File descriptor for the port */
+int actuator_ardrone2_raw_fd; /**< File descriptor for the port */
+
+#define ARDRONE_GPIO_PORT 0x32524
+
+#define ARDRONE_GPIO_PIN_MOTOR1 171
+#define ARDRONE_GPIO_PIN_MOTOR2 172
+#define ARDRONE_GPIO_PIN_MOTOR3 173
+#define ARDRONE_GPIO_PIN_MOTOR4 174
+
+#define ARDRONE_GPIO_PIN_IRQ_FLIPFLOP 175
+#define ARDRONE_GPIO_PIN_IRQ_INPUT 176
+
+uint32_t led_hw_values;
+
+static inline void actuators_ardrone_reset_flipflop(void)
+{
+ gpio_setup_output(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP);
+ gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP);
+ int32_t stop = sys_time.nb_sec + 2;
+ while (sys_time.nb_sec < stop);
+ gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP);
+}
+
+
void actuators_ardrone_init(void)
{
+ led_hw_values = 0;
+
//open mot port
- mot_fd = open("/dev/ttyO0", O_RDWR | O_NOCTTY | O_NDELAY);
- if (mot_fd == -1)
+ actuator_ardrone2_raw_fd = open("/dev/ttyO0", O_RDWR | O_NOCTTY | O_NDELAY);
+ if (actuator_ardrone2_raw_fd == -1)
{
perror("open_port: Unable to open /dev/ttyO0 - ");
return;
}
- fcntl(mot_fd, F_SETFL, 0); //read calls are non blocking
- fcntl(mot_fd, F_GETFL, 0);
+ fcntl(actuator_ardrone2_raw_fd, F_SETFL, 0); //read calls are non blocking
+ fcntl(actuator_ardrone2_raw_fd, F_GETFL, 0);
//set port options
struct termios options;
//Get the current options for the port
- tcgetattr(mot_fd, &options);
+ tcgetattr(actuator_ardrone2_raw_fd, &options);
//Set the baud rates to 115200
cfsetispeed(&options, B115200);
cfsetospeed(&options, B115200);
@@ -78,37 +106,41 @@ void actuators_ardrone_init(void)
options.c_oflag &= ~OPOST; //clear output options (raw output)
//Set the new options for the port
- tcsetattr(mot_fd, TCSANOW, &options);
+ tcsetattr(actuator_ardrone2_raw_fd, TCSANOW, &options);
//reset IRQ flipflop - on error 106 read 1, this code resets 106 to 0
- gpio_set(106,-1);
- gpio_set(107,0);
- gpio_set(107,1);
+ gpio_setup_input(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_INPUT);
+ actuators_ardrone_reset_flipflop();
- //all select lines inactive
- gpio_set(68,1);
- gpio_set(69,1);
- gpio_set(70,1);
- gpio_set(71,1);
+
+ //all select lines active
+ gpio_setup_output(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR1);
+ gpio_setup_output(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR2);
+ gpio_setup_output(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR3);
+ gpio_setup_output(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR4);
+ gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR1);
+ gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR2);
+ gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR3);
+ gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR4);
//configure motors
uint8_t reply[256];
for(int m=0;m<4;m++) {
- gpio_set(68+m,-1);
+ gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR1 + m);
actuators_ardrone_cmd(0xe0,reply,2);
if(reply[0]!=0xe0 || reply[1]!=0x00)
{
printf("motor%d cmd=0x%02x reply=0x%02x\n",m+1,(int)reply[0],(int)reply[1]);
}
actuators_ardrone_cmd(m+1,reply,1);
- gpio_set(68+m,1);
+ gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR1 + m);
}
//all select lines active
- gpio_set(68,-1);
- gpio_set(69,-1);
- gpio_set(70,-1);
- gpio_set(71,-1);
+ gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR1);
+ gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR2);
+ gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR3);
+ gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR4);
//start multicast
actuators_ardrone_cmd(0xa0,reply,1);
@@ -117,23 +149,89 @@ void actuators_ardrone_init(void)
actuators_ardrone_cmd(0xa0,reply,1);
actuators_ardrone_cmd(0xa0,reply,1);
- //reset IRQ flipflop - on error 106 read 1, this code resets 106 to 0
- gpio_set(106,-1);
- gpio_set(107,0);
- gpio_set(107,1);
+ //reset IRQ flipflop - on error 176 reads 1, this code resets 176 to 0
+ gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP);
+ gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP);
- //all leds green
-// actuators_ardrone_set_leds(MOT_LEDGREEN, MOT_LEDGREEN, MOT_LEDGREEN, MOT_LEDGREEN);
+ // Left Red, Right Green
+ actuators_ardrone_set_leds(MOT_LEDRED,MOT_LEDGREEN, MOT_LEDGREEN, MOT_LEDRED);
}
int actuators_ardrone_cmd(uint8_t cmd, uint8_t *reply, int replylen) {
- write(mot_fd, &cmd, 1);
- return read(mot_fd, reply, replylen);
+ if (full_write(actuator_ardrone2_raw_fd, &cmd, 1) < 0)
+ {
+ perror("actuators_ardrone_cmd: write failed");
+ return -1;
+ }
+ return full_read(actuator_ardrone2_raw_fd, reply, replylen);
+}
+
+#include "autopilot.h"
+
+void actuators_ardrone_motor_status(void);
+void actuators_ardrone_motor_status(void)
+{
+ static bool_t last_motor_on = FALSE;
+
+ // Reset Flipflop sequence
+ static bool_t reset_flipflop_counter = 0;
+ if (reset_flipflop_counter > 0)
+ {
+ reset_flipflop_counter--;
+
+ if (reset_flipflop_counter == 10)
+ {
+ // Reset flipflop
+ gpio_setup_output(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP);
+ gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP);
+ }
+ else if (reset_flipflop_counter == 1)
+ {
+ // Listen to IRQ again
+ gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP);
+ }
+ return;
+ }
+
+ // If a motor IRQ line is set
+ if (gpio_get(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_IRQ_INPUT) == 1)
+ {
+ if (autopilot_motors_on)
+ {
+ if (last_motor_on)
+ {
+ // Tell paparazzi that one motor has stalled
+ autopilot_set_motors_on(FALSE);
+ }
+ else
+ {
+ // Toggle Flipflop reset so motors can be re-enabled
+ reset_flipflop_counter = 20;
+ }
+
+ }
+ }
+ last_motor_on = autopilot_motors_on;
+
+}
+
+#define BIT_NUMBER(VAL,BIT) (((VAL)>>BIT)&0x03)
+
+void actuators_ardrone_led_run(void);
+void actuators_ardrone_led_run(void)
+{
+ static uint32_t previous_led_hw_values = 0x00;
+ if (previous_led_hw_values != led_hw_values)
+ {
+ previous_led_hw_values = led_hw_values;
+ actuators_ardrone_set_leds(BIT_NUMBER(led_hw_values,0), BIT_NUMBER(led_hw_values,2), BIT_NUMBER(led_hw_values,4), BIT_NUMBER(led_hw_values,6) );
+ }
}
void actuators_ardrone_commit(void)
{
actuators_ardrone_set_pwm(actuators_pwm_values[0], actuators_pwm_values[1], actuators_pwm_values[2], actuators_pwm_values[3]);
+ RunOnceEvery(100,actuators_ardrone_motor_status());
}
/**
@@ -148,22 +246,39 @@ void actuators_ardrone_set_pwm(uint16_t pwm0, uint16_t pwm1, uint16_t pwm2, uint
cmd[2] = ((pwm1&0x1ff)<<3) | ((pwm2&0x1ff)>>6);
cmd[3] = ((pwm2&0x1ff)<<2) | ((pwm3&0x1ff)>>7);
cmd[4] = ((pwm3&0x1ff)<<1);
- write(mot_fd, cmd, 5);
+ full_write(actuator_ardrone2_raw_fd, cmd, 5);
+ RunOnceEvery(20,actuators_ardrone_led_run());
}
/**
* Write LED command
- * cmd = 011grgrg rgrxxxxx (this is ardrone1 format, we need ardrone2 format)
+ * cmd = 011rrrr0 000gggg0 (this is ardrone1 format, we need ardrone2 format)
+ *
+ *
+ * led0 = RearLeft
+ * led1 = RearRight
+ * led2 = FrontRight
+ * led3 = FrontLeft
*/
+
void actuators_ardrone_set_leds(uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3)
{
uint8_t cmd[2];
- cmd[0]=0x60 | ((led0&3)<<3) | ((led1&3)<<1) | ((led2&3)>>1);
- cmd[1]=((led2&3)<<7) | ((led3&3)<<5);
- write(mot_fd, cmd, 2);
+
+ led0 &= 0x03;
+ led1 &= 0x03;
+ led2 &= 0x03;
+ led3 &= 0x03;
+
+ printf("LEDS: %d %d %d %d \n", led0, led1, led2, led3);
+
+ cmd[0]=0x60 | ((led0&1)<<4) | ((led1&1)<<3) | ((led2&1)<<2) | ((led3&1) <<1);
+ cmd[1]=((led0&2)<<3) | ((led1&2)<<2) | ((led2&2)<<1) | ((led3&2)<<0);
+
+ full_write(actuator_ardrone2_raw_fd, cmd, 2);
}
void actuators_ardrone_close(void)
{
- close(mot_fd);
+ close(actuator_ardrone2_raw_fd);
}
diff --git a/sw/airborne/boards/ardrone/actuators_ardrone2_raw.h b/sw/airborne/boards/ardrone/actuators_ardrone2_raw.h
index 2a66ba5dde..c564dd0ecc 100644
--- a/sw/airborne/boards/ardrone/actuators_ardrone2_raw.h
+++ b/sw/airborne/boards/ardrone/actuators_ardrone2_raw.h
@@ -53,6 +53,7 @@ uint16_t actuators_pwm_values[ACTUATORS_ARDRONE_NB];
extern void actuators_ardrone_commit(void);
extern void actuators_ardrone_init(void);
+
int actuators_ardrone_cmd(uint8_t cmd, uint8_t *reply, int replylen);
void actuators_ardrone_set_pwm(uint16_t pwm0, uint16_t pwm1, uint16_t pwm2, uint16_t pwm3);
void actuators_ardrone_set_leds(uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3);
diff --git a/sw/airborne/boards/ardrone/at_com.c b/sw/airborne/boards/ardrone/at_com.c
index b2c11e4d0b..eba448ee13 100644
--- a/sw/airborne/boards/ardrone/at_com.c
+++ b/sw/airborne/boards/ardrone/at_com.c
@@ -133,10 +133,14 @@ void init_at_config(void) {
}
//Recieve a navdata packet
-void at_com_recieve_navdata(unsigned char* buffer) {
- int l;
- recvfrom(navdata_socket, buffer, ARDRONE_NAVDATA_BUFFER_SIZE, 0x0,
- (struct sockaddr *) &from, (socklen_t *) &l);
+int at_com_recieve_navdata(unsigned char* buffer) {
+ int l = sizeof(from);
+ int n;
+ // FIXME(ben): not clear why recvfrom() and not recv() is used.
+ n = recvfrom(navdata_socket, buffer, ARDRONE_NAVDATA_BUFFER_SIZE, 0x0,
+ (struct sockaddr *) &from, (socklen_t *) &l);
+
+ return n;
}
//Send an AT command
diff --git a/sw/airborne/boards/ardrone/at_com.h b/sw/airborne/boards/ardrone/at_com.h
index f2b04185b2..79084315ef 100644
--- a/sw/airborne/boards/ardrone/at_com.h
+++ b/sw/airborne/boards/ardrone/at_com.h
@@ -202,7 +202,7 @@ typedef struct _navdata_gps_t {
//External functions
extern void init_at_com(void);
-extern void at_com_recieve_navdata(unsigned char* buffer);
+extern int at_com_recieve_navdata(unsigned char* buffer);
extern void at_com_send_config(char* key, char* value);
extern void at_com_send_ftrim(void);
extern void at_com_send_ref(int bits);
diff --git a/sw/airborne/boards/ardrone/baro_board.c b/sw/airborne/boards/ardrone/baro_board.c
index 7526ac485e..9b05dad93a 100644
--- a/sw/airborne/boards/ardrone/baro_board.c
+++ b/sw/airborne/boards/ardrone/baro_board.c
@@ -1,5 +1,5 @@
/*
- * Copyright (C) 2012 TU Delft Quatrotor Team 1
+ * Copyright (C) 2013 TU Delft Quatrotor Team 1
*
* This file is part of Paparazzi.
*
@@ -23,32 +23,66 @@
* @file boards/ardrone/baro_board.c
* Paparazzi AR Drone 2 Baro Sensor implementation:.
*
- * These functions are mostly empty because of the calibration and calculations
- * done by the Parrot Navigation board.
+ * Based on BMP180 implementation by C. de Wagter.
*/
#include "subsystems/sensors/baro.h"
#include "baro_board.h"
+#include "navdata.h"
struct Baro baro;
+#define BMP180_OSS 0 // Parrot ARDrone uses no oversampling
+
void baro_init(void) {
baro.status = BS_UNINITIALIZED;
- baro.absolute = 0;
- baro.differential = 0;
- baro_data_available = 0;
+ baro.absolute = 0;
+ baro.differential = 0;
}
-void baro_periodic(void) {
- baro.status = BS_RUNNING;
- if(navdata_baro_available == 1) {
- navdata_baro_available = 0;
-// baro.absolute = navdata->pressure; // When this is un-commented the ardrone gets a pressure
- // TODO do the right calculations for the right absolute pressure
- baro.absolute = 0;
- baro_data_available = TRUE;
+static inline int32_t baro_apply_calibration(int32_t raw)
+{
+ int32_t b6 = ((int32_t)baro_calibration.b5) - 4000L;
+ int32_t x1 = (((int32_t)baro_calibration.b2) * (b6 * b6 >> 12)) >> 11;
+ int32_t x2 = ((int32_t)baro_calibration.ac2) * b6 >> 11;
+ int32_t x3 = x1 + x2;
+ int32_t b3 = (((((int32_t)baro_calibration.ac1) * 4 + x3) << BMP180_OSS) + 2)/4;
+ x1 = ((int32_t)baro_calibration.ac3) * b6 >> 13;
+ x2 = (((int32_t)baro_calibration.b1) * (b6 * b6 >> 12)) >> 16;
+ x3 = ((x1 + x2) + 2) >> 2;
+ uint32_t b4 = (((int32_t)baro_calibration.ac4) * (uint32_t) (x3 + 32768L)) >> 15;
+ uint32_t b7 = (raw - b3) * (50000L >> BMP180_OSS);
+ int32_t p = b7 < 0x80000000L ? (b7 * 2) / b4 : (b7 / b4) * 2;
+ x1 = (p >> 8) * (p >> 8);
+ x1 = (x1 * 3038UL) >> 16;
+ x2 = (-7357L * p) >> 16;
+ int32_t press = p + ((x1 + x2 + 3791L) >> 4);
+ // Zero at sealevel
+ return press;
+}
+
+static inline int32_t baro_apply_calibration_temp(int32_t tmp_raw)
+{
+ int32_t x1 = ((tmp_raw - ((int32_t)baro_calibration.ac6)) * ((int32_t)baro_calibration.ac5)) >> 15;
+ int32_t x2 = (((int32_t)baro_calibration.mc) << 11) / (x1 + ((int32_t)baro_calibration.md));
+ baro_calibration.b5 = x1 + x2;
+ return (baro_calibration.b5 + 8) >> 4;
+}
+
+void baro_periodic(void)
+{
+}
+
+void process_ardrone_baro(void)
+{
+ if(baro.status == BS_RUNNING) {
+ // first read temperature because pressure calibration depends on temperature
+ baro.differential = baro_apply_calibration_temp(navdata.temperature_pressure); // We store the temperature in Baro-Diff
+ baro.absolute = baro_apply_calibration(navdata.pressure);
}
else {
- baro_data_available = FALSE;
+ if (baro_calibrated == TRUE) {
+ baro.status = BS_RUNNING;
+ }
}
}
diff --git a/sw/airborne/boards/ardrone/baro_board.h b/sw/airborne/boards/ardrone/baro_board.h
index 7233f8b437..d98a771108 100644
--- a/sw/airborne/boards/ardrone/baro_board.h
+++ b/sw/airborne/boards/ardrone/baro_board.h
@@ -33,10 +33,14 @@
#if BOARD_HAS_BARO
#include "navdata.h"
-int baro_data_available;
+void process_ardrone_baro(void);
+
static inline void baro_event(void (*b_abs_handler)(void), void (*b_diff_handler)(void)){
- if (baro_data_available) {
+ if (navdata_baro_available)
+ {
+ navdata_baro_available = 0;
+ process_ardrone_baro();
b_abs_handler();
}
}
diff --git a/sw/airborne/boards/ardrone/electrical_raw.c b/sw/airborne/boards/ardrone/electrical_raw.c
new file mode 100644
index 0000000000..6ffc6a8f5a
--- /dev/null
+++ b/sw/airborne/boards/ardrone/electrical_raw.c
@@ -0,0 +1,143 @@
+/*
+ *
+ * Copyright (C) 2009-2013 The Paparazzi Team
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/**
+ * @file boards/ardrone/electrical_raw.c
+ * arch specific electrical status readings
+ */
+
+#include "electrical_raw.h"
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include "i2c-dev.h"
+#include "subsystems/commands.h"
+#include "generated/airframe.h"
+
+struct Electrical electrical;
+
+#if defined ADC_CHANNEL_VSUPPLY || defined ADC_CHANNEL_CURRENT || defined MILLIAMP_AT_FULL_THROTTLE
+static struct {
+#ifdef ADC_CHANNEL_VSUPPLY
+ struct adc_buf vsupply_adc_buf;
+#endif
+#ifdef ADC_CHANNEL_CURRENT
+ struct adc_buf current_adc_buf;
+#endif
+#ifdef MILLIAMP_AT_FULL_THROTTLE
+ float nonlin_factor;
+#endif
+} electrical_priv;
+#endif
+
+#if defined COMMAND_THROTTLE
+#define COMMAND_CURRENT_ESTIMATION COMMAND_THROTTLE
+#elif defined COMMAND_THRUST
+#define COMMAND_CURRENT_ESTIMATION COMMAND_THRUST
+#endif
+
+#ifndef CURRENT_ESTIMATION_NONLINEARITY
+#define CURRENT_ESTIMATION_NONLINEARITY 1.2
+#endif
+
+int fd;
+
+void electrical_init(void) {
+ // First we try to kill the program.elf if it is running (done here because initializes first)
+ int ret = system("killall -9 program.elf");
+ (void) ret;
+
+ // Initialize 12c device for power
+ fd = open( "/dev/i2c-1", O_RDWR );
+ if ( ioctl( fd, I2C_SLAVE_FORCE, 0x4a) < 0 ) {
+ fprintf( stderr, "Failed to set slave address: %m\n" );
+ }
+
+ electrical_setup();
+ electrical_priv.nonlin_factor = CURRENT_ESTIMATION_NONLINEARITY;
+}
+
+void electrical_setup(void) {
+ // Turn on MADC in CTRL1
+ if( i2c_smbus_write_byte_data( fd, 0x00, 0x01)) {
+ fprintf( stderr, "Failed to write to I2C device. 1\n" );
+ }
+ // Select ADCIN0 for conversion in SW1SELECT_LSB
+ if( i2c_smbus_write_byte_data( fd, 0x06, 0xff)){
+ fprintf( stderr, "Failed to write to I2C device. 2\n" );
+ }
+ // Select ADCIN12 for conversion in SW1SELECT_MSB
+ if( i2c_smbus_write_byte_data( fd, 0x07, 0xff)) {
+ fprintf( stderr, "Failed to write to I2C device. 3\n" );
+ }
+ // Setup register for averaging
+ if( i2c_smbus_write_byte_data( fd, 0x08, 0xff)) {
+ fprintf( stderr, "Failed to write to I2C device. 4\n" );
+ }
+ // Start all channel conversion by setting bit 5 to one in CTRL_SW1
+ if( i2c_smbus_write_byte_data( fd, 0x12, 0x20)) {
+ fprintf( stderr, "Failed to write to I2C device. 5\n" );
+ }
+}
+
+void electrical_periodic(void) {
+
+ electrical_setup();
+
+ unsigned char lsb, msb;
+ lsb = i2c_smbus_read_byte_data(fd, 0x37);
+ msb = i2c_smbus_read_byte_data(fd, 0x38);
+
+ int raw_voltage = (lsb >> 6) | (msb << 2);
+
+ // we know from spec sheet that ADCIN0 has no prescaler
+ // so that the max voltage range is 1.5 volt
+ // multiply by ten to get decivolts
+
+ //from raw measurement we got quite a lineair response
+ //9.0V=662, 9.5V=698, 10.0V=737,10.5V=774, 11.0V=811, 11.5V=848, 12.0V=886, 12.5V=923
+ //leading to our 0.13595166 magic number for decivolts conversion
+ electrical.vsupply = raw_voltage*0.13595166;
+
+ /*
+ * 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_CURRENT_ESTIMATION]) / ((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));
+}
diff --git a/sw/airborne/arch/omap/subsystems/electrical/electrical_arch.h b/sw/airborne/boards/ardrone/electrical_raw.h
similarity index 85%
rename from sw/airborne/arch/omap/subsystems/electrical/electrical_arch.h
rename to sw/airborne/boards/ardrone/electrical_raw.h
index 450acec3e2..9db3a59c29 100644
--- a/sw/airborne/arch/omap/subsystems/electrical/electrical_arch.h
+++ b/sw/airborne/boards/ardrone/electrical_raw.h
@@ -22,13 +22,15 @@
*/
/**
- * @file arch/omap/subsystems/electrical/electrical_arch.h
+ * @file boards/ardrone/electrical_raw.h
* arch specific electrical status readings
*/
-#ifndef ELECTRICAL_ARCH_H_
-#define ELECTRICAL_ARCH_H_
+#ifndef ELECTRICAL_RAW_H_
+#define ELECTRICAL_RAW_H_
#include "subsystems/electrical.h"
-#endif /* ELECTRICAL_ARCH_H_ */
+void electrical_setup(void);
+
+#endif /* ELECTRICAL_RAW_H_ */
diff --git a/sw/airborne/boards/ardrone/gpio_ardrone.c b/sw/airborne/boards/ardrone/gpio_ardrone.c
index 3a08b1c267..363ca87686 100644
--- a/sw/airborne/boards/ardrone/gpio_ardrone.c
+++ b/sw/airborne/boards/ardrone/gpio_ardrone.c
@@ -22,18 +22,112 @@
* ardrone GPIO driver
*/
-#include
-#include
-#include "gpio_ardrone.h"
+#ifdef ARDRONE2_RAW
-//val=0 -> set gpio output lo
-//val=1 -> set gpio output hi
-//val=-1 -> set gpio as input (output hi-Z)
-int gpio_set(int nr,int val)
+#include /* File control definitions */
+#include /* Error number definitions */
+#include
+
+#include "mcu_periph/gpio.h"
+
+#define GPIO_MAGIC 'p'
+#define GPIO_DIRECTION _IOW(GPIO_MAGIC, 0, struct gpio_direction)
+#define GPIO_READ _IOWR(GPIO_MAGIC, 1, struct gpio_data)
+#define GPIO_WRITE _IOW(GPIO_MAGIC, 2, struct gpio_data)
+int gpiofp = 0;
+
+struct gpio_data {
+ int pin;
+ int value;
+};
+
+enum gpio_mode {
+ GPIO_INPUT = 0, //!< Pin configured for input
+ GPIO_OUTPUT_LOW, //!< Pin configured for output with low level
+ GPIO_OUTPUT_HIGH, //!< Pin configured for output with high level
+};
+
+struct gpio_direction {
+ int pin;
+ enum gpio_mode mode;
+};
+
+
+void gpio_set(uint32_t port, uint16_t pin)
{
- char cmdline[200];
- if(val<0) sprintf(cmdline,"/usr/sbin/gpio %d -d i",nr);
- else if(val>0) sprintf(cmdline,"/usr/sbin/gpio %d -d ho 1",nr);
- else sprintf(cmdline,"/usr/sbin/gpio %d -d ho 0",nr);
- return system(cmdline);
+ if (port != 0x32524) return; // protect ardrone board from unauthorized use
+ struct gpio_data data;
+ // Open the device if not open
+ if (gpiofp == 0)
+ gpiofp = open("/dev/gpio",O_RDWR);
+
+ // Read the GPIO value
+ data.pin = pin;
+ data.value = 1;
+ ioctl(gpiofp, GPIO_WRITE, &data);
}
+
+
+void gpio_clear(uint32_t port, uint16_t pin)
+{
+ if (port != 0x32524) return; // protect ardrone board from unauthorized use
+ struct gpio_data data;
+ // Open the device if not open
+ if (gpiofp == 0)
+ gpiofp = open("/dev/gpio",O_RDWR);
+
+ // Read the GPIO value
+ data.pin = pin;
+ data.value = 0;
+ ioctl(gpiofp, GPIO_WRITE, &data);
+}
+
+
+void gpio_setup_input(uint32_t port, uint16_t pin)
+{
+ if (port != 0x32524) return; // protect ardrone board from unauthorized use
+ struct gpio_direction dir;
+ // Open the device if not open
+ if (gpiofp == 0)
+ gpiofp = open("/dev/gpio",O_RDWR);
+
+ // Read the GPIO value
+ dir.pin = pin;
+ dir.mode = GPIO_INPUT;
+ ioctl(gpiofp, GPIO_DIRECTION, &dir);
+}
+
+
+void gpio_setup_output(uint32_t port, uint16_t pin)
+{
+ /*
+ if (port != 0x32524) return; // protect ardrone board from unauthorized use
+ struct gpio_direction dir;
+ // Open the device if not open
+ if (gpiofp == 0)
+ gpiofp = open("/dev/gpio",O_RDWR);
+
+ // Read the GPIO value
+ dir.pin = pin;
+ dir.mode = GPIO_OUTPUT_LOW;
+ ioctl(gpiofp, GPIO_DIRECTION, &dir);
+ */
+}
+
+
+
+uint16_t gpio_get(uint32_t port, uint16_t pin)
+{
+ if (port != 0x32524) return 0; // protect ardrone board from unauthorized use
+ struct gpio_data data;
+ // Open the device if not open
+ if (gpiofp == 0)
+ gpiofp = open("/dev/gpio",O_RDWR);
+
+ // Read the GPIO value
+ data.pin = pin;
+ ioctl(gpiofp, GPIO_READ, &data);
+ return data.value;
+}
+
+#endif
diff --git a/sw/airborne/boards/ardrone/gpio_ardrone.h b/sw/airborne/boards/ardrone/gpio_ardrone.h
deleted file mode 100644
index 55563f2e09..0000000000
--- a/sw/airborne/boards/ardrone/gpio_ardrone.h
+++ /dev/null
@@ -1,33 +0,0 @@
-/*
- * Copyright (C) 2011 Hugo Perquin - http://blog.perquin.com
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
- * MA 02110-1301 USA.
- */
-
-/**
- * @file boards/ardrone/gpio_ardrone.h
- * ardrone GPIO driver
- */
-
-#ifndef GPIO_ARDRONE_H
-#define GPIO_ARDRONE_H
-
-//val=0 -> set gpio output lo
-//val=1 -> set gpio output hi
-//val=-1 -> set gpio as input (output hi-Z)
-int gpio_set(int nr,int val);
-
-#endif /* GPIO_ARDRONE_H */
diff --git a/sw/airborne/boards/ardrone/navdata.c b/sw/airborne/boards/ardrone/navdata.c
index 5653554b63..5736b89ca0 100644
--- a/sw/airborne/boards/ardrone/navdata.c
+++ b/sw/airborne/boards/ardrone/navdata.c
@@ -1,5 +1,5 @@
/*
- * Copyright (C) 2012 Dino Hensen, Vincent van Hoek
+ * Copyright (C) 2013 Dino Hensen, Vincent van Hoek
*
* This file is part of Paparazzi.
*
@@ -34,9 +34,71 @@
#include
#include
#include
-#include "navdata.h"
+#include
+#include
-int nav_fd;
+#include "std.h"
+#include "navdata.h"
+#include "subsystems/ins.h"
+
+#define NAVDATA_PACKET_SIZE 60
+#define NAVDATA_START_BYTE 0x3a
+
+static inline bool_t acquire_baro_calibration(void);
+static void navdata_cropbuffer(int cropsize);
+
+navdata_port nav_port;
+static int nav_fd = 0;
+static int16_t previousUltrasoundHeight;
+measures_t navdata;
+
+#include "subsystems/sonar.h"
+uint16_t sonar_meas = 0;
+
+
+// FIXME(ben): there must be a better home for these
+ssize_t full_write(int fd, const uint8_t *buf, size_t count)
+{
+ size_t written = 0;
+
+ while(written < count)
+ {
+ ssize_t n = write(fd, buf + written, count - written);
+ if (n < 0)
+ {
+ if (errno == EAGAIN || errno == EWOULDBLOCK)
+ continue;
+ return n;
+ }
+ written += n;
+ }
+ return written;
+}
+
+ssize_t full_read(int fd, uint8_t *buf, size_t count)
+{
+ // Apologies for illiteracy, but we can't overload |read|.
+ size_t readed = 0;
+
+ while(readed < count)
+ {
+ ssize_t n = read(fd, buf + readed, count - readed);
+ if (n < 0)
+ {
+ if (errno == EAGAIN || errno == EWOULDBLOCK)
+ continue;
+ return n;
+ }
+ readed += n;
+ }
+ return readed;
+}
+
+static void navdata_write(const uint8_t *buf, size_t count)
+{
+ if (full_write(nav_fd, buf, count) < 0)
+ perror("navdata_write: Write failed");
+}
#if DOWNLINK
#include "subsystems/datalink/telemetry.h"
@@ -74,17 +136,15 @@ static void send_navdata(void) {
}
#endif
-int navdata_init()
+bool_t navdata_init()
{
- port = malloc(sizeof(navdata_port));
+ if (nav_fd <= 0) {
+ nav_fd = open("/dev/ttyO1", O_RDWR | O_NOCTTY | O_NONBLOCK);
- nav_fd = open("/dev/ttyO1", O_RDWR | O_NOCTTY | O_NONBLOCK);
- if (nav_fd == -1)
- {
- perror("navdata_init: Unable to open /dev/ttyO1 - ");
- return 1;
- } else {
- port->isOpen = 1;
+ if (nav_fd == -1) {
+ perror("navdata_init: Unable to open /dev/ttyO1 - ");
+ return FALSE;
+ }
}
fcntl(nav_fd, F_SETFL, 0); //read calls are non blocking
@@ -106,153 +166,250 @@ int navdata_init()
// stop acquisition
uint8_t cmd=0x02;
- write(nav_fd, &cmd, 1);
+ navdata_write(&cmd, 1);
+
+ // read some potential dirt (retry alot of times)
+ char tmp[100];
+ for(int i = 0; i < 100; i++) {
+ uint16_t dirt = read(nav_fd, tmp, sizeof tmp);
+ (void) dirt;
+
+ cmd=0x02;
+ navdata_write(&cmd, 1);
+ usleep(200);
+ }
+
+ baro_calibrated = FALSE;
+ if(!acquire_baro_calibration())
+ return FALSE;
// start acquisition
- cmd=0x01;
- write(nav_fd, &cmd, 1);
+ cmd = 0x01;
+ navdata_write(&cmd, 1);
- navdata = malloc(sizeof(measures_t));
- navdata_imu_available = 0;
- navdata_baro_available = 0;
-
- port->bytesRead = 0;
- port->totalBytesRead = 0;
- port->packetsRead = 0;
- port->isInitialized = 1;
+ navdata_imu_available = FALSE;
+ navdata_baro_available = FALSE;
previousUltrasoundHeight = 0;
+ nav_port.checksum_errors = 0;
+ nav_port.bytesRead = 0;
+ nav_port.totalBytesRead = 0;
+ nav_port.packetsRead = 0;
+ nav_port.isInitialized = TRUE;
#if DOWNLINK
register_periodic_telemetry(DefaultPeriodic, "ARDRONE_NAVDATA", send_navdata);
#endif
- return 0;
+ return TRUE;
}
-void navdata_close()
+static inline bool_t acquire_baro_calibration(void)
{
- port->isOpen = 0;
- close(nav_fd);
+ // start baro calibration acquisition
+ uint8_t cmd=0x17; // send cmd 23
+ navdata_write(&cmd, 1);
+
+ uint8_t calibBuffer[22];
+
+ if (full_read(nav_fd, calibBuffer, sizeof calibBuffer) < 0)
+ {
+ perror("acquire_baro_calibration: read failed");
+ return FALSE;
+ }
+
+ baro_calibration.ac1 = calibBuffer[0] << 8 | calibBuffer[1];
+ baro_calibration.ac2 = calibBuffer[2] << 8 | calibBuffer[3];
+ baro_calibration.ac3 = calibBuffer[4] << 8 | calibBuffer[5];
+ baro_calibration.ac4 = calibBuffer[6] << 8 | calibBuffer[7];
+ baro_calibration.ac5 = calibBuffer[8] << 8 | calibBuffer[9];
+ baro_calibration.ac6 = calibBuffer[10] << 8 | calibBuffer[11];
+ baro_calibration.b1 = calibBuffer[12] << 8 | calibBuffer[13];
+ baro_calibration.b2 = calibBuffer[14] << 8 | calibBuffer[15];
+ baro_calibration.mb = calibBuffer[16] << 8 | calibBuffer[17];
+ baro_calibration.mc = calibBuffer[18] << 8 | calibBuffer[19];
+ baro_calibration.md = calibBuffer[20] << 8 | calibBuffer[21];
+
+ printf("Calibration AC1: %d\n", baro_calibration.ac1);
+ printf("Calibration AC2: %d\n", baro_calibration.ac2);
+ printf("Calibration AC3: %d\n", baro_calibration.ac3);
+ printf("Calibration AC4: %d\n", baro_calibration.ac4);
+ printf("Calibration AC5: %d\n", baro_calibration.ac5);
+ printf("Calibration AC6: %d\n", baro_calibration.ac6);
+
+ printf("Calibration B1: %d\n", baro_calibration.b1);
+ printf("Calibration B2: %d\n", baro_calibration.b2);
+
+ printf("Calibration MB: %d\n", baro_calibration.mb);
+ printf("Calibration MC: %d\n", baro_calibration.mc);
+ printf("Calibration MD: %d\n", baro_calibration.md);
+
+ baro_calibrated = TRUE;
+ return TRUE;
}
void navdata_read()
{
- int newbytes = 0;
-
- if (port->isInitialized != 1)
- navdata_init();
-
- if (port->isOpen != 1)
- return;
-
- newbytes = read(nav_fd, port->buffer+port->bytesRead, NAVDATA_BUFFER_SIZE-port->bytesRead);
+ int newbytes = read(nav_fd, nav_port.buffer+nav_port.bytesRead, NAVDATA_BUFFER_SIZE-nav_port.bytesRead);
// because non-blocking read returns -1 when no bytes available
if (newbytes > 0)
{
- port->bytesRead += newbytes;
- port->totalBytesRead += newbytes;
+ nav_port.bytesRead += newbytes;
+ nav_port.totalBytesRead += newbytes;
+ }
+}
+
+static void baro_update_logic(void)
+{
+ static int32_t lastpressval = 0;
+ static uint16_t lasttempval = 0;
+ static uint8_t temp_or_press_was_updated_last = 0; // 0 = press, so we now wait for temp, 1 = temp so we now wait for press
+
+ static int sync_errors;
+
+ if (temp_or_press_was_updated_last == 0) // Last update was press so we are now waiting for temp
+ {
+ // temp was updated
+ temp_or_press_was_updated_last = TRUE;
+
+ // This means that press must remain constant
+ if (lastpressval != 0)
+ {
+ // If pressure was updated: this is a sync error
+ if (lastpressval != navdata.pressure)
+ {
+ // wait for temp again
+ temp_or_press_was_updated_last = FALSE;
+ sync_errors++;
+ navdata_baro_available = TRUE;
+ }
+ }
+ }
+ else
+ {
+ // press was updated
+ temp_or_press_was_updated_last = FALSE;
+
+ // This means that temp must remain constant
+ if (lasttempval != 0)
+ {
+ // If temp was updated: this is a sync error
+ if (lasttempval != navdata.temperature_pressure)
+ {
+ // wait for press again
+ temp_or_press_was_updated_last = TRUE;
+ sync_errors++;
+ }
+ }
+
+ navdata_baro_available = TRUE;
}
+ lastpressval = navdata.pressure;
+ lasttempval = navdata.temperature_pressure;
}
void navdata_update()
{
+ static bool_t last_checksum_wrong = FALSE;
+ // Check if initialized
+ if (!nav_port.isInitialized) {
+ navdata_init();
+ return;
+ }
+
+ // Start reading
navdata_read();
// while there is something interesting to do...
- while (port->bytesRead >= 60)
+ while (nav_port.bytesRead >= NAVDATA_PACKET_SIZE)
{
- if (port->buffer[0] == NAVDATA_START_BYTE)
+ if (nav_port.buffer[0] == NAVDATA_START_BYTE)
{
- // if checksum is OK
- if ( 1 ) // we dont know how to calculate the checksum
-// if ( navdata_checksum() == 0 )
- {
- memcpy(navdata, port->buffer, NAVDATA_PACKET_SIZE);
- navdata_imu_available = 1;
- navdata_baro_available = 1;
- port->packetsRead++;
-// printf("CCRC=%d, GCRC=%d, error=%d\n", crc, navdata->chksum, abs(crc-navdata->chksum));
- navdata_getHeight();
+ assert(sizeof navdata == NAVDATA_PACKET_SIZE);
+ memcpy(&navdata, nav_port.buffer, NAVDATA_PACKET_SIZE);
+
+ // Calculating the checksum
+ uint16_t checksum = 0;
+ for(int i = 2; i < NAVDATA_PACKET_SIZE-2; i += 2) {
+ checksum += nav_port.buffer[i] + (nav_port.buffer[i+1] << 8);
}
- navdata_CropBuffer(60);
+
+ // When checksum is incorrect
+ if(navdata.chksum != checksum) {
+ printf("Checksum error [calculated: %d] [packet: %d] [diff: %d]\n",checksum , navdata.chksum, checksum-navdata.chksum);
+ nav_port.checksum_errors++;
+ }
+
+ // When we already dropped a packet or checksum is correct
+ if(last_checksum_wrong || navdata.chksum == checksum) {
+ // Invert byte order so that TELEMETRY works better
+ uint8_t tmp;
+ uint8_t* p = (uint8_t*) &(navdata.pressure);
+ tmp = p[0];
+ p[0] = p[1];
+ p[1] = tmp;
+ p = (uint8_t*) &(navdata.temperature_pressure);
+ tmp = p[0];
+ p[0] = p[1];
+ p[1] = tmp;
+
+ baro_update_logic();
+
+#ifdef USE_SONAR
+ if (navdata.ultrasound < 10000)
+ {
+ sonar_meas = navdata.ultrasound;
+ ins_update_sonar();
+
+ }
+#endif
+
+
+ navdata_imu_available = TRUE;
+ last_checksum_wrong = FALSE;
+ nav_port.packetsRead++;
+ }
+
+ // Crop the buffer
+ navdata_cropbuffer(NAVDATA_PACKET_SIZE);
}
else
{
// find start byte, copy all data from startbyte to buffer origin, update bytesread
uint8_t * pint;
- pint = memchr(port->buffer, NAVDATA_START_BYTE, port->bytesRead);
+ pint = memchr(nav_port.buffer, NAVDATA_START_BYTE, nav_port.bytesRead);
if (pint != NULL) {
- port->bytesRead -= pint - port->buffer;
- navdata_CropBuffer(pint - port->buffer);
+ navdata_cropbuffer(pint - nav_port.buffer);
} else {
// if the start byte was not found, it means there is junk in the buffer
- port->bytesRead = 0;
+ nav_port.bytesRead = 0;
}
}
}
}
-void navdata_CropBuffer(int cropsize)
-{
- if (port->bytesRead - cropsize < 0) {
- // TODO think about why the amount of bytes read minus the cropsize gets below zero
- printf("BytesRead - Cropsize may not be below zero...");
- return;
- }
-
- memmove(port->buffer, port->buffer+cropsize, NAVDATA_BUFFER_SIZE-cropsize);
- port->bytesRead -= cropsize;
-}
-
-int16_t navdata_getHeight() {
-
- if (navdata->ultrasound > 10000) {
+int16_t navdata_height(void) {
+ if (navdata.ultrasound > 10000) {
return previousUltrasoundHeight;
}
int16_t ultrasoundHeight = 0;
-
- ultrasoundHeight = (navdata->ultrasound - 880) / 26.553;
-
+ ultrasoundHeight = (navdata.ultrasound - 880) / 26.553;
previousUltrasoundHeight = ultrasoundHeight;
-
return ultrasoundHeight;
}
-// The checksum should be calculated here: we don't know the algorithm
-uint16_t navdata_checksum() {
- navdata_cks = 0;
- navdata_cks += navdata->nu_trame;
- navdata_cks += navdata->ax;
- navdata_cks += navdata->ay;
- navdata_cks += navdata->az;
- navdata_cks += navdata->vx;
- navdata_cks += navdata->vy;
- navdata_cks += navdata->vz;
- navdata_cks += navdata->temperature_acc;
- navdata_cks += navdata->temperature_gyro;
- navdata_cks += navdata->ultrasound;
- navdata_cks += navdata->us_debut_echo;
- navdata_cks += navdata->us_fin_echo;
- navdata_cks += navdata->us_association_echo;
- navdata_cks += navdata->us_distance_echo;
- navdata_cks += navdata->us_curve_time;
- navdata_cks += navdata->us_curve_value;
- navdata_cks += navdata->us_curve_ref;
- navdata_cks += navdata->nb_echo;
- navdata_cks += navdata->sum_echo;
- navdata_cks += navdata->gradient;
- navdata_cks += navdata->flag_echo_ini;
- navdata_cks += navdata->pressure;
- navdata_cks += navdata->temperature_pressure;
- navdata_cks += navdata->mx;
- navdata_cks += navdata->my;
- navdata_cks += navdata->mz;
-// navdata_cks += navdata->chksum;
+static void navdata_cropbuffer(int cropsize)
+{
+ if (nav_port.bytesRead - cropsize < 0) {
+ // TODO think about why the amount of bytes read minus the cropsize gets below zero
+ printf("BytesRead(=%d) - Cropsize(=%d) may not be below zero. port->buffer=%p\n", nav_port.bytesRead, cropsize, nav_port.buffer);
+ return;
+ }
- return 0; // we dont know how to calculate the checksum
+ memmove(nav_port.buffer, nav_port.buffer+cropsize, NAVDATA_BUFFER_SIZE-cropsize);
+ nav_port.bytesRead -= cropsize;
}
diff --git a/sw/airborne/boards/ardrone/navdata.h b/sw/airborne/boards/ardrone/navdata.h
index cf149b2196..f41b52a9c0 100644
--- a/sw/airborne/boards/ardrone/navdata.h
+++ b/sw/airborne/boards/ardrone/navdata.h
@@ -1,5 +1,5 @@
/*
- * Copyright (C) 2012 Dino Hensen, Vincent van Hoek
+ * Copyright (C) 2013 Dino Hensen, Vincent van Hoek
*
* This file is part of Paparazzi.
*
@@ -31,19 +31,7 @@
#define NAVDATA_H_
#include
-
-#define NAVDATA_PACKET_SIZE 60
-#define NAVDATA_BUFFER_SIZE 80
-#define NAVDATA_START_BYTE 0x3a
-
-typedef struct {
- uint8_t isInitialized;
- uint8_t isOpen;
- uint16_t bytesRead;
- uint32_t totalBytesRead;
- uint32_t packetsRead;
- uint8_t buffer[NAVDATA_BUFFER_SIZE];
-} navdata_port;
+#include
typedef struct
{
@@ -78,32 +66,60 @@ typedef struct
uint16_t flag_echo_ini;
- int32_t pressure; //long
- int16_t temperature_pressure;
+ int32_t pressure;
+ uint16_t temperature_pressure;
- int16_t mx;
int16_t my;
+ int16_t mx;
int16_t mz;
uint16_t chksum;
} __attribute__ ((packed)) measures_t;
-measures_t* navdata;
+struct bmp180_baro_calibration
+{
+ 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;
+};
+
+#define NAVDATA_BUFFER_SIZE 80
+typedef struct {
+ uint8_t isInitialized;
+ uint16_t bytesRead;
+ uint32_t totalBytesRead;
+ uint32_t packetsRead;
+ uint32_t checksum_errors;
+ uint8_t buffer[NAVDATA_BUFFER_SIZE];
+} navdata_port;
+
+extern measures_t navdata;
+extern navdata_port nav_port;
+struct bmp180_baro_calibration baro_calibration;
navdata_port* port;
uint16_t navdata_cks;
uint8_t navdata_imu_available;
uint8_t navdata_baro_available;
-int16_t previousUltrasoundHeight;
-
-int navdata_init(void);
-void navdata_close(void);
+uint8_t baro_calibrated;
+bool_t navdata_init(void);
void navdata_read(void);
void navdata_update(void);
-void navdata_CropBuffer(int cropsize);
+int16_t navdata_height(void);
-uint16_t navdata_checksum(void);
-int16_t navdata_getHeight(void);
+ssize_t full_write(int fd, const uint8_t *buf, size_t count);
+ssize_t full_read(int fd, uint8_t *buf, size_t count);
#endif /* NAVDATA_H_ */
diff --git a/sw/airborne/boards/krooz/baro_board.c b/sw/airborne/boards/krooz/baro_board.c
deleted file mode 100644
index 7b531ec97b..0000000000
--- a/sw/airborne/boards/krooz/baro_board.c
+++ /dev/null
@@ -1,32 +0,0 @@
-
-#include "subsystems/sensors/baro.h"
-#include "baro_board.h"
-/*
-#include "subsystems/datalink/downlink.h"
-#include "mcu_periph/uart.h"
-#include "mcu_periph/sys_time.h"
-*/
-
-struct Baro baro;
-
-void baro_init(void) {
- baro_ms5611_init();
-}
-
-void baro_periodic(void) {
- static uint8_t cnt;
- switch(cnt) {
- case 0:
- baro_ms5611_periodic();
- cnt++;
- break;
- case 1:
- baro_ms5611_d1();
- cnt++;
- break;
- case 2:
- baro_ms5611_d2();
- cnt = 0;
- break;
- }
-}
diff --git a/sw/airborne/boards/krooz/baro_board.h b/sw/airborne/boards/krooz/baro_board.h
index b73f33d90d..5f47ad650e 100644
--- a/sw/airborne/boards/krooz/baro_board.h
+++ b/sw/airborne/boards/krooz/baro_board.h
@@ -1,30 +1,14 @@
/*
- * board specific fonctions for the KroozSD board
+ * board specific interface for the KroozSD board
*
+ * It uses the subsystems/sensors/baro_ms5611_i2c.c driver
*/
#ifndef BOARDS_KROOZ_BARO_H
#define BOARDS_KROOZ_BARO_H
-#include "std.h"
-#include "mcu_periph/i2c.h"
-#include "modules/sensors/baro_ms5611_i2c.h"
-#include "math/pprz_algebra_int.h"
-
-//#include "led.h"
-
-static inline void baro_event(void (*b_abs_handler)(void), void (*b_diff_handler)(void))
-{
- baro_ms5611_event();
- if(baro_ms5611_valid) {
- baro.status = BS_RUNNING;
- baro.absolute = (int32_t)baroms;
- b_abs_handler();
- baro_ms5611_valid = FALSE;
- }
-}
-
-#define BaroEvent(_b_abs_handler, _b_diff_handler) baro_event(_b_abs_handler,_b_diff_handler)
+extern void baro_event(void (*b_abs_handler)(void));
+#define BaroEvent(_b_abs_handler, _b_diff_handler) baro_event(_b_abs_handler)
#endif /* BOARDS_KROOZ_SD_BARO_H */
diff --git a/sw/airborne/boards/krooz/imu_krooz.c b/sw/airborne/boards/krooz/imu_krooz.c
index 3aad6a7803..8d369be25a 100644
--- a/sw/airborne/boards/krooz/imu_krooz.c
+++ b/sw/airborne/boards/krooz/imu_krooz.c
@@ -33,6 +33,7 @@
#include "subsystems/imu/imu_krooz_sd_arch.h"
#include "mcu_periph/i2c.h"
#include "led.h"
+#include "filters/median_filter.h"
// Downlink
#include "mcu_periph/uart.h"
@@ -63,10 +64,13 @@ PRINT_CONFIG_VAR(KROOZ_ACCEL_RANGE)
struct ImuKrooz imu_krooz;
-#if KROOZ_USE_MEDIAN_FILTER
-#include "filters/median_filter.h"
-struct MedianFilter3Int median_gyro, median_accel, median_mag;
+#if IMU_KROOZ_USE_GYRO_MEDIAN_FILTER
+struct MedianFilter3Int median_gyro;
#endif
+#if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
+struct MedianFilter3Int median_accel;
+#endif
+struct MedianFilter3Int median_mag;
void imu_impl_init( void )
{
@@ -82,12 +86,14 @@ void imu_impl_init( void )
hmc58xx_init(&imu_krooz.hmc, &(IMU_KROOZ_I2C_DEV), HMC58XX_ADDR);
-#if KROOZ_USE_MEDIAN_FILTER
// Init median filters
+#if IMU_KROOZ_USE_GYRO_MEDIAN_FILTER
InitMedianFilterRatesInt(median_gyro);
- InitMedianFilterVect3Int(median_accel);
- InitMedianFilterVect3Int(median_mag);
#endif
+#if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
+ InitMedianFilterVect3Int(median_accel);
+#endif
+ InitMedianFilterVect3Int(median_mag);
RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0);
VECT3_ASSIGN(imu_krooz.accel_sum, 0, 0, 0);
@@ -97,6 +103,9 @@ void imu_impl_init( void )
imu_krooz.acc_valid = FALSE;
imu_krooz.mag_valid = FALSE;
+ imu_krooz.hmc_eoc = FALSE;
+ imu_krooz.mpu_eoc = FALSE;
+
imu_krooz_sd_arch_init();
}
@@ -110,14 +119,25 @@ void imu_periodic( void )
hmc58xx_start_configure(&imu_krooz.hmc);
if (imu_krooz.meas_nb) {
- RATES_ASSIGN(imu.gyro_unscaled, imu_krooz.rates_sum.q / imu_krooz.meas_nb, imu_krooz.rates_sum.p / imu_krooz.meas_nb, imu_krooz.rates_sum.r / imu_krooz.meas_nb);
-#if KROOZ_USE_MEDIAN_FILTER
+ RATES_ASSIGN(imu.gyro_unscaled, -imu_krooz.rates_sum.q / imu_krooz.meas_nb, imu_krooz.rates_sum.p / imu_krooz.meas_nb, imu_krooz.rates_sum.r / imu_krooz.meas_nb);
+#if IMU_KROOZ_USE_GYRO_MEDIAN_FILTER
UpdateMedianFilterRatesInt(median_gyro, imu.gyro_unscaled);
#endif
- VECT3_ASSIGN(imu.accel_unscaled, imu_krooz.accel_sum.y / imu_krooz.meas_nb, imu_krooz.accel_sum.x / imu_krooz.meas_nb, imu_krooz.accel_sum.z / imu_krooz.meas_nb);
-#if KROOZ_USE_MEDIAN_FILTER
+ VECT3_ASSIGN(imu.accel_unscaled, -imu_krooz.accel_sum.y / imu_krooz.meas_nb, imu_krooz.accel_sum.x / imu_krooz.meas_nb, imu_krooz.accel_sum.z / imu_krooz.meas_nb);
+#if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER
UpdateMedianFilterVect3Int(median_accel, imu.accel_unscaled);
#endif
+
+ RATES_SMUL(imu_krooz.gyro_filtered, imu_krooz.gyro_filtered, IMU_KROOZ_GYRO_AVG_FILTER);
+ RATES_ADD(imu_krooz.gyro_filtered, imu.gyro_unscaled);
+ RATES_SDIV(imu_krooz.gyro_filtered, imu_krooz.gyro_filtered, (IMU_KROOZ_GYRO_AVG_FILTER + 1));
+ RATES_COPY(imu.gyro_unscaled, imu_krooz.gyro_filtered);
+
+ VECT3_SMUL(imu_krooz.accel_filtered, imu_krooz.accel_filtered, IMU_KROOZ_ACCEL_AVG_FILTER);
+ VECT3_ADD(imu_krooz.accel_filtered, imu.accel_unscaled);
+ VECT3_SDIV(imu_krooz.accel_filtered, imu_krooz.accel_filtered, (IMU_KROOZ_ACCEL_AVG_FILTER + 1));
+ VECT3_COPY(imu.accel_unscaled, imu_krooz.accel_filtered);
+
RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0);
VECT3_ASSIGN(imu_krooz.accel_sum, 0, 0, 0);
imu_krooz.meas_nb = 0;
@@ -138,6 +158,11 @@ void imu_krooz_downlink_raw( void )
void imu_krooz_event( void )
{
+ if (imu_krooz.mpu_eoc) {
+ mpu60x0_i2c_read(&imu_krooz.mpu);
+ imu_krooz.mpu_eoc = FALSE;
+ }
+
// If the MPU6050 I2C transaction has succeeded: convert the data
mpu60x0_i2c_event(&imu_krooz.mpu);
if (imu_krooz.mpu.data_available) {
@@ -147,13 +172,16 @@ void imu_krooz_event( void )
imu_krooz.mpu.data_available = FALSE;
}
+ if (imu_krooz.hmc_eoc) {
+ hmc58xx_read(&imu_krooz.hmc);
+ imu_krooz.hmc_eoc = FALSE;
+ }
+
// If the HMC5883 I2C transaction has succeeded: convert the data
hmc58xx_event(&imu_krooz.hmc);
if (imu_krooz.hmc.data_available) {
- VECT3_COPY(imu.mag_unscaled, imu_krooz.hmc.data.vect);
-#if KROOZ_USE_MEDIAN_FILTER
+ VECT3_ASSIGN(imu.mag_unscaled, imu_krooz.hmc.data.vect.y, -imu_krooz.hmc.data.vect.x, imu_krooz.hmc.data.vect.z);
UpdateMedianFilterVect3Int(median_mag, imu.mag_unscaled);
-#endif
imu_krooz.hmc.data_available = FALSE;
imu_krooz.mag_valid = TRUE;
}
diff --git a/sw/airborne/boards/krooz/imu_krooz.h b/sw/airborne/boards/krooz/imu_krooz.h
index 74afaf00f3..565e48a838 100644
--- a/sw/airborne/boards/krooz/imu_krooz.h
+++ b/sw/airborne/boards/krooz/imu_krooz.h
@@ -39,12 +39,12 @@
// Default configuration
#if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN
-#define IMU_GYRO_P_SIGN -1
+#define IMU_GYRO_P_SIGN 1
#define IMU_GYRO_Q_SIGN 1
#define IMU_GYRO_R_SIGN 1
#endif
#if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN
-#define IMU_ACCEL_X_SIGN -1
+#define IMU_ACCEL_X_SIGN 1
#define IMU_ACCEL_Y_SIGN 1
#define IMU_ACCEL_Z_SIGN 1
#endif
@@ -100,15 +100,26 @@
#define IMU_ACCEL_Z_NEUTRAL 0
#endif
+#ifndef IMU_KROOZ_GYRO_AVG_FILTER
+#define IMU_KROOZ_GYRO_AVG_FILTER 5
+#endif
+#ifndef IMU_KROOZ_ACCEL_AVG_FILTER
+#define IMU_KROOZ_ACCEL_AVG_FILTER 10
+#endif
+
struct ImuKrooz {
volatile bool_t gyr_valid;
volatile bool_t acc_valid;
volatile bool_t mag_valid;
+ volatile bool_t mpu_eoc;
+ volatile bool_t hmc_eoc;
struct Mpu60x0_I2c mpu;
struct Hmc58xx hmc;
struct Int32Rates rates_sum;
struct Int32Vect3 accel_sum;
volatile uint8_t meas_nb;
+ struct Int32Vect3 accel_filtered;
+ struct Int32Rates gyro_filtered;
};
extern struct ImuKrooz imu_krooz;
diff --git a/sw/airborne/boards/krooz_sd.h b/sw/airborne/boards/krooz_sd.h
index dc24cbda4a..1ecd270753 100644
--- a/sw/airborne/boards/krooz_sd.h
+++ b/sw/airborne/boards/krooz_sd.h
@@ -126,7 +126,7 @@
/* I2C mapping */
#define I2C1_GPIO_PORT GPIOB
#define I2C1_GPIO_SCL GPIO8
-#define I2C1_GPIO_SDA GPIO7
+#define I2C1_GPIO_SDA GPIO9
#define I2C2_GPIO_PORT GPIOB
#define I2C2_GPIO_SCL GPIO10
diff --git a/sw/airborne/boards/lia/baro_board.h b/sw/airborne/boards/lia/baro_board.h
index d65c11d266..59782132c8 100644
--- a/sw/airborne/boards/lia/baro_board.h
+++ b/sw/airborne/boards/lia/baro_board.h
@@ -9,8 +9,8 @@
#include "std.h"
-extern void baro_event(void (*b_abs_handler)(void), void (*b_diff_handler)(void));
+extern void baro_event(void (*b_abs_handler)(void));
-#define BaroEvent(_b_abs_handler, _b_diff_handler) baro_event(_b_abs_handler,_b_diff_handler)
+#define BaroEvent(_b_abs_handler, _b_diff_handler) baro_event(_b_abs_handler)
#endif /* BOARDS_LIA_BARO_H */
diff --git a/sw/airborne/boards/lisa_m/baro_board.c b/sw/airborne/boards/lisa_m/baro_board.c
index b51743c30a..9c09b97d90 100644
--- a/sw/airborne/boards/lisa_m/baro_board.c
+++ b/sw/airborne/boards/lisa_m/baro_board.c
@@ -1,195 +1,81 @@
+/*
+ * Copyright (C) 2013 Felix Ruess
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/** @file boards/lisa_m/baro_board.c
+ * Baro board interface for Bosch BMP085 on LisaM I2C2 with EOC check.
+ */
#include "subsystems/sensors/baro.h"
-#include
+#include "peripherals/bmp085_regs.h"
+#include
+
+#include "led.h"
struct Baro baro;
-struct BaroBoard baro_board;
-struct i2c_transaction baro_trans;
-struct bmp085_baro_calibration calibration;
+struct Bmp085 baro_bmp085;
-#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)
+static bool_t baro_eoc(void)
{
- baro_trans.buf[0] = addr;
- baro_trans.buf[1] = value;
-
- i2c_transmit(&i2c2, &baro_trans, BMP085_ADDR, 2);
-
- // FIXME, no while loops without timeout!!
- while (baro_trans.status == I2CTransPending || baro_trans.status == I2CTransRunning);
-}
-
-static inline void bmp085_read_reg16(uint8_t addr)
-{
- baro_trans.buf[0] = addr;
- i2c_transceive(&i2c2, &baro_trans, BMP085_ADDR, 1, 2);
-}
-
-static inline int16_t bmp085_read_reg16_blocking(uint8_t addr, uint32_t timeout)
-{
- uint32_t time = 0;
-
- bmp085_read_reg16(addr);
-
- while (baro_trans.status == I2CTransPending || baro_trans.status == I2CTransRunning) {
- if ((time == timeout) && (timeout != 0)) {
- return 0; /* Timeout of the i2c read */
- } else {
- time++;
- }
- }
-
- return ((baro_trans.buf[0] << 8) | baro_trans.buf[1]);
-}
-
-static inline void bmp085_read_reg24(uint8_t addr)
-{
- baro_trans.buf[0] = addr;
- i2c_transceive(&i2c2, &baro_trans, BMP085_ADDR, 1, 3);
-}
-
-static void bmp085_baro_read_calibration(void)
-{
- calibration.ac1 = bmp085_read_reg16_blocking(0xAA, 10000); // AC1
- calibration.ac2 = bmp085_read_reg16_blocking(0xAC, 10000); // AC2
- calibration.ac3 = bmp085_read_reg16_blocking(0xAE, 10000); // AC3
- calibration.ac4 = bmp085_read_reg16_blocking(0xB0, 10000); // AC4
- calibration.ac5 = bmp085_read_reg16_blocking(0xB2, 10000); // AC5
- calibration.ac6 = bmp085_read_reg16_blocking(0xB4, 10000); // AC6
- calibration.b1 = bmp085_read_reg16_blocking(0xB6, 10000); // B1
- calibration.b2 = bmp085_read_reg16_blocking(0xB8, 10000); // B2
- calibration.mb = bmp085_read_reg16_blocking(0xBA, 10000); // MB
- calibration.mc = bmp085_read_reg16_blocking(0xBC, 10000); // MC
- calibration.md = bmp085_read_reg16_blocking(0xBE, 10000); // MD
+ return gpio_get(GPIOB, GPIO0);
}
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?) */
+ bmp085_init(&baro_bmp085, &i2c2, BMP085_SLAVE_ADDR);
+
+ /* setup eoc check function */
+ baro_bmp085.eoc = &baro_eoc;
+
gpio_clear(GPIOB, GPIO0);
gpio_set_mode(GPIOB, GPIO_MODE_INPUT,
GPIO_CNF_INPUT_PULL_UPDOWN, GPIO0);
}
-static inline int baro_eoc(void)
-{
- return gpio_get(GPIOB, GPIO0);
-}
-
-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;
+ if (baro_bmp085.initialized) {
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;
+ bmp085_periodic(&baro_bmp085);
}
-
+ else
+ bmp085_read_eeprom_calib(&baro_bmp085);
}
-void baro_board_send_reset(void) {
- // This is a NOP at the moment
-}
-// Apply temp calibration and sensor calibration to raw measurement to get Pa (from BMP085 datasheet)
-static int32_t baro_apply_calibration(int32_t raw)
+
+void baro_event(void (*b_abs_handler)(void))
{
- 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);
-}
+ bmp085_event(&baro_bmp085);
-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();
- }
+ if (baro_bmp085.data_available) {
+ baro.absolute = baro_bmp085.pressure;
+ b_abs_handler();
+ baro_bmp085.data_available = FALSE;
+
+#ifdef ROTORCRAFT_BARO_LED
+ RunOnceEvery(10,LED_TOGGLE(ROTORCRAFT_BARO_LED));
+#endif
}
}
diff --git a/sw/airborne/boards/lisa_m/baro_board.h b/sw/airborne/boards/lisa_m/baro_board.h
index 42014ddeee..b84a11a7da 100644
--- a/sw/airborne/boards/lisa_m/baro_board.h
+++ b/sw/airborne/boards/lisa_m/baro_board.h
@@ -12,56 +12,15 @@
// for right now we abuse this file for the ms5611 baro on aspirin as well
#if !BARO_MS5611_I2C && !BARO_MS5611
-#include "mcu_periph/i2c.h"
+#include "peripherals/bmp085.h"
-// absolute addr
-#define BMP085_ADDR 0xEE
-// Over sample setting (0-3)
-#define BMP085_OSS 3
+extern struct Bmp085 baro_bmp085;
-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);
#endif // !BARO_MS5611_xx
-extern void baro_event(void (*b_abs_handler)(void), void (*b_diff_handler)(void));
+extern void baro_event(void (*b_abs_handler)(void));
-#define BaroEvent(_b_abs_handler, _b_diff_handler) baro_event(_b_abs_handler,_b_diff_handler)
+#define BaroEvent(_b_abs_handler, _b_diff_handler) baro_event(_b_abs_handler)
#endif /* BOARDS_LISA_M_BARO_H */
diff --git a/sw/airborne/boards/lisa_m/baro_ms5611_i2c.c b/sw/airborne/boards/lisa_m/baro_ms5611_i2c.c
deleted file mode 100644
index 44a2c7fa64..0000000000
--- a/sw/airborne/boards/lisa_m/baro_ms5611_i2c.c
+++ /dev/null
@@ -1,203 +0,0 @@
-/**
- * Measurement Specialties (Intersema) MS5611-01BA pressure/temperature sensor interface for I2C
- *
- * Edit by: Michal Podhradsky, michal.podhradsky@aggiemail.usu.edu
- * Utah State University, http://aggieair.usu.edu/
- */
-#include "subsystems/sensors/baro.h"
-#include "peripherals/ms5611.h"
-#include "led.h"
-#include "std.h"
-#include "mcu_periph/sys_time.h"
-
-#include "mcu_periph/i2c.h"
-#ifndef MS5611_I2C_DEV
-#define MS5611_I2C_DEV i2c2
-#endif
-
-#ifdef DEBUG
-#include "mcu_periph/uart.h"
-#include "messages.h"
-#include "subsystems/datalink/downlink.h"
-#endif
-
-struct Baro baro;
-struct i2c_transaction ms5611_trans;
-uint8_t ms5611_status;
-int32_t prom_cnt;
-uint16_t ms5611_c[PROM_NB];
-uint32_t ms5611_d1, ms5611_d2;
-float fbaroms, ftempms;
-
-static int8_t baro_ms5611_crc(uint16_t* prom) {
- int32_t i, j;
- uint32_t res = 0;
- uint8_t crc = prom[7] & 0xF;
- prom[7] &= 0xFF00;
- for (i = 0; i < 16; i++) {
- if (i & 1) res ^= ((prom[i>>1]) & 0x00FF);
- else res ^= (prom[i>>1]>>8);
- for (j = 8; j > 0; j--) {
- if (res & 0x8000) res ^= 0x1800;
- res <<= 1;
- }
- }
- prom[7] |= crc;
- if (crc == ((res >> 12) & 0xF)) return 0;
- else return -1;
-}
-
-void baro_init(void) {
- ms5611_status = MS5611_UNINIT;
- baro.status = BS_UNINITIALIZED;
- prom_cnt = 0;
-}
-
-void baro_periodic(void) {
- if (sys_time.nb_sec > 1) {
- if (ms5611_status == MS5611_IDLE) {
- /* start D1 conversion */
- ms5611_status = MS5611_CONV_D1;
- ms5611_trans.buf[0] = MS5611_START_CONV_D1;
- i2c_transmit(&MS5611_I2C_DEV, &ms5611_trans, MS5611_SLAVE_ADDR, 1);
- #ifdef DEBUG
- RunOnceEvery(60, { DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice,
- &ms5611_c[0], &ms5611_c[1], &ms5611_c[2], &ms5611_c[3],
- &ms5611_c[4], &ms5611_c[5], &ms5611_c[6], &ms5611_c[7]);});
- #endif
- }
- else if (ms5611_status == MS5611_CONV_D1) {
- /* assume D1 conversion is done */
- ms5611_status = MS5611_CONV_D1_OK;
- }
- else if (ms5611_status == MS5611_CONV_D1_OK) {
- /* read D1 adc */
- ms5611_status = MS5611_ADC_D1;
- ms5611_trans.buf[0] = MS5611_ADC_READ;
- i2c_transceive(&MS5611_I2C_DEV, &ms5611_trans, MS5611_SLAVE_ADDR, 1, 3);
- }
- else if (ms5611_status == MS5611_CONV_D2) {
- /* assume D2 conversion is done */
- ms5611_status = MS5611_CONV_D2_OK;
- }
- else if (ms5611_status == MS5611_CONV_D2_OK) {
- /* read D2 adc */
- ms5611_status = MS5611_ADC_D2;
- ms5611_trans.buf[0] = MS5611_ADC_READ;
- i2c_transceive(&MS5611_I2C_DEV, &ms5611_trans, MS5611_SLAVE_ADDR, 1, 3);
- }
- else if (ms5611_status == MS5611_UNINIT) {
- /* reset sensor */
- ms5611_status = MS5611_RESET;
- ms5611_trans.buf[0] = MS5611_SOFT_RESET;
- i2c_transmit(&MS5611_I2C_DEV, &ms5611_trans, MS5611_SLAVE_ADDR, 1);
- }
- else if (ms5611_status == MS5611_RESET_OK) {
- /* start getting prom data */
- ms5611_status = MS5611_PROM;
- ms5611_trans.buf[0] = MS5611_PROM_READ | (prom_cnt << 1);
- i2c_transceive(&MS5611_I2C_DEV, &ms5611_trans, MS5611_SLAVE_ADDR, 1, 2);
- }
- }
-}
-
-void baro_event(void (*b_abs_handler)(void), void (*b_diff_handler)(void)){
- if (ms5611_trans.status == I2CTransSuccess) {
- #ifdef ROTORCRAFT_BARO_LED
- RunOnceEvery(10,LED_TOGGLE(ROTORCRAFT_BARO_LED));
- #endif
- switch (ms5611_status) {
-
- case MS5611_RESET:
- ms5611_status = MS5611_RESET_OK;
- ms5611_trans.status = I2CTransDone;
- break;
-
- case MS5611_PROM:
- /* read prom data */
- ms5611_c[prom_cnt++] = (ms5611_trans.buf[0] << 8) | ms5611_trans.buf[1];
- if (prom_cnt < PROM_NB) {//8 bytes at PROM
- /* get next prom data */
- ms5611_trans.buf[0] = MS5611_PROM_READ | (prom_cnt << 1);
- i2c_transceive(&MS5611_I2C_DEV, &ms5611_trans, MS5611_SLAVE_ADDR, 1, 2);
- }
- else {
- /* done reading prom */
- ms5611_trans.status = I2CTransDone;
- /* check prom crc */
- if (baro_ms5611_crc(ms5611_c) == 0) {
- ms5611_status = MS5611_IDLE;
- baro.status = BS_RUNNING;
- }
- else {
- /* checksum error, try again */
- baro_init();
- }
- }
- break;
-
- case MS5611_ADC_D1:
- /* read D1 (pressure) */
- ms5611_d1 = (ms5611_trans.buf[0] << 16) |
- (ms5611_trans.buf[1] << 8) |
- ms5611_trans.buf[2];
- /* start D2 conversion */
- ms5611_status = MS5611_CONV_D2;
- ms5611_trans.buf[0] = MS5611_START_CONV_D2;
- i2c_transmit(&MS5611_I2C_DEV, &ms5611_trans, MS5611_SLAVE_ADDR, 1);
- break;
-
- case MS5611_ADC_D2: {
- int64_t dt, baroms, tempms, off, sens, t2, off2, sens2;
- /* read D2 (temperature) */
- ms5611_d2 = (ms5611_trans.buf[0] << 16) |
- (ms5611_trans.buf[1] << 8) |
- ms5611_trans.buf[2];
- ms5611_status = MS5611_IDLE;
- ms5611_trans.status = I2CTransDone;
-
- /* difference between actual and ref temperature */
- dt = ms5611_d2 - (int64_t)ms5611_c[5] * (1<<8);
- /* actual temperature */
- tempms = 2000 + ((int64_t)dt * ms5611_c[6]) / (1<<23);
- /* offset at actual temperature */
- off = ((int64_t)ms5611_c[2] * (1<<16)) + ((int64_t)ms5611_c[4] * dt) / (1<<7);
- /* sensitivity at actual temperature */
- sens = ((int64_t)ms5611_c[1] * (1<<15)) + ((int64_t)ms5611_c[3] * dt) / (1<<8);
- /* second order temperature compensation */
- if (tempms < 2000) {
- t2 = (dt*dt) / (1<<31);
- off2 = 5 * ((int64_t)(tempms-2000)*(tempms-2000)) / (1<<1);
- sens2 = 5 * ((int64_t)(tempms-2000)*(tempms-2000)) / (1<<2);
- if (tempms < -1500) {
- off2 = off2 + 7 * (int64_t)(tempms+1500)*(tempms+1500);
- sens2 = sens2 + 11 * ((int64_t)(tempms+1500)*(tempms+1500)) / (1<<1);
- }
- tempms = tempms - t2;
- off = off - off2;
- sens = sens - sens2;
- }
- /* temperature compensated pressure */
- baroms = (((int64_t)ms5611_d1 * sens) / (1<<21) - off) / (1<<15);
-
- /* Update baro structure */
- baro.absolute = (int32_t)baroms;
-
- b_abs_handler();
- b_diff_handler();
-
- #ifdef DEBUG
- ftempms = tempms / 100.;
- fbaroms = baroms / 100.;
- DOWNLINK_SEND_BARO_MS5611(DefaultChannel, DefaultDevice,
- &ms5611_d1, &ms5611_d2, &fbaroms, &ftempms, &baro.status);
- #endif
- break;
- }
-
- default:
- ms5611_trans.status = I2CTransDone;
- break;
- }
- }
-}
diff --git a/sw/airborne/boards/lisa_m/baro_ms5611_spi.c b/sw/airborne/boards/lisa_m/baro_ms5611_spi.c
deleted file mode 100644
index 7be72f8326..0000000000
--- a/sw/airborne/boards/lisa_m/baro_ms5611_spi.c
+++ /dev/null
@@ -1,223 +0,0 @@
-/**
- * Measurement Specialties (Intersema) MS5611-01BA pressure/temperature sensor interface for I2C
- *
- * Edit by: Michal Podhradsky, michal.podhradsky@aggiemail.usu.edu
- * Utah State University, http://aggieair.usu.edu/
- */
-#include "subsystems/sensors/baro.h"
-#include "peripherals/ms5611.h"
-#include "led.h"
-#include "std.h"
-#include "mcu_periph/sys_time.h"
-
-#include "mcu_periph/spi.h"
-#ifndef MS5611_SPI_DEV
-#define MS5611_SPI_DEV spi2
-#endif
-#define MS5611_BUFFER_LENGTH 4
-
-#ifdef DEBUG
-#include "mcu_periph/uart.h"
-#include "messages.h"
-#include "subsystems/datalink/downlink.h"
-#endif
-
-struct Baro baro;
-struct spi_transaction ms5611_trans;
-uint8_t ms5611_status;
-int32_t prom_cnt;
-uint16_t ms5611_c[PROM_NB];
-uint32_t ms5611_d1, ms5611_d2;
-float fbaroms, ftempms;
-volatile uint8_t input_buf_ms5611[MS5611_BUFFER_LENGTH];
-volatile uint8_t output_buf_ms5611[MS5611_BUFFER_LENGTH];
-static void trans_cb_ms5611( struct spi_transaction *trans );
-
-static int8_t baro_ms5611_crc(uint16_t* prom) {
- int32_t i, j;
- uint32_t res = 0;
- uint8_t crc = prom[7] & 0xF;
- prom[7] &= 0xFF00;
- for (i = 0; i < 16; i++) {
- if (i & 1) res ^= ((prom[i>>1]) & 0x00FF);
- else res ^= (prom[i>>1]>>8);
- for (j = 8; j > 0; j--) {
- if (res & 0x8000) res ^= 0x1800;
- res <<= 1;
- }
- }
- prom[7] |= crc;
- if (crc == ((res >> 12) & 0xF)) return 0;
- else return -1;
-}
-
-static void trans_cb_ms5611( struct spi_transaction *trans ) {
-#ifdef ROTORCRAFT_BARO_LED
- RunOnceEvery(10,LED_TOGGLE(ROTORCRAFT_BARO_LED));
-#endif
-}
-
-void baro_init(void) {
- ms5611_trans.select = SPISelectUnselect;
- ms5611_trans.cpol = SPICpolIdleHigh;
- ms5611_trans.cpha = SPICphaEdge2;
- ms5611_trans.dss = SPIDss8bit;
- ms5611_trans.bitorder = SPIMSBFirst;
- ms5611_trans.cdiv = SPIDiv64;
- ms5611_trans.slave_idx = MS5611_SLAVE_DEV;
- ms5611_trans.output_length = MS5611_BUFFER_LENGTH;
- ms5611_trans.input_length = MS5611_BUFFER_LENGTH;
- ms5611_trans.after_cb = trans_cb_ms5611;
- ms5611_trans.input_buf = &input_buf_ms5611[0];
- ms5611_trans.output_buf = &input_buf_ms5611[0];
-
- ms5611_status = MS5611_UNINIT;
- baro.status = BS_UNINITIALIZED;
- prom_cnt = 0;
-}
-
-void baro_periodic(void) {
- if (sys_time.nb_sec > 1) {
- if (ms5611_status == MS5611_IDLE) {
- /* start D1 conversion */
- ms5611_status = MS5611_CONV_D1;
- ms5611_trans.output_buf[0] = MS5611_START_CONV_D1;
- spi_submit(&(MS5611_SPI_DEV), &ms5611_trans);
- #ifdef DEBUG
- RunOnceEvery(300, { DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice,
- &ms5611_c[0], &ms5611_c[1], &ms5611_c[2], &ms5611_c[3],
- &ms5611_c[4], &ms5611_c[5], &ms5611_c[6], &ms5611_c[7]);});
- #endif
- }
- else if (ms5611_status == MS5611_CONV_D1) {
- /* assume D1 conversion is done */
- ms5611_status = MS5611_CONV_D1_OK;
- }
- else if (ms5611_status == MS5611_CONV_D1_OK) {
- /* read D1 adc */
- ms5611_status = MS5611_ADC_D1;
- ms5611_trans.output_buf[0] = MS5611_ADC_READ;
- spi_submit(&(MS5611_SPI_DEV), &ms5611_trans);
- }
- else if (ms5611_status == MS5611_CONV_D2) {
- /* assume D2 conversion is done */
- ms5611_status = MS5611_CONV_D2_OK;
- }
- else if (ms5611_status == MS5611_CONV_D2_OK) {
- /* read D2 adc */
- ms5611_status = MS5611_ADC_D2;
- ms5611_trans.output_buf[0] = MS5611_ADC_READ;
- spi_submit(&(MS5611_SPI_DEV), &ms5611_trans);
- }
- else if (ms5611_status == MS5611_UNINIT) {
- /* reset sensor */
- ms5611_status = MS5611_RESET;
- ms5611_trans.output_buf[0] = MS5611_SOFT_RESET;
- spi_submit(&(MS5611_SPI_DEV), &ms5611_trans);
- }
- else if (ms5611_status == MS5611_RESET_OK) {
- /* start getting prom data */
- ms5611_status = MS5611_PROM;
- ms5611_trans.output_buf[0] = MS5611_PROM_READ | (prom_cnt << 1);
- spi_submit(&(MS5611_SPI_DEV), &ms5611_trans);
- }
- }
-}
-
-void baro_event(void (*b_abs_handler)(void), void (*b_diff_handler)(void)){
- if (ms5611_trans.status == SPITransSuccess) {
- switch (ms5611_status) {
-
- case MS5611_RESET:
- ms5611_status = MS5611_RESET_OK;
- ms5611_trans.status = SPITransDone;
- break;
-
- case MS5611_PROM:
- /* read prom data */
- ms5611_c[prom_cnt++] = (ms5611_trans.input_buf[1] << 8) | ms5611_trans.input_buf[2];
- if (prom_cnt < PROM_NB) {//8 bytes at PROM
- /* get next prom data */
- ms5611_trans.output_buf[0] = MS5611_PROM_READ | (prom_cnt << 1);
- spi_submit(&(MS5611_SPI_DEV), &ms5611_trans);
- }
- else {
- /* done reading prom */
- ms5611_trans.status = SPITransDone;
- /* check prom crc */
- if (baro_ms5611_crc(ms5611_c) == 0) {
- ms5611_status = MS5611_IDLE;
- baro.status = BS_RUNNING;
- }
- else {
- /* checksum error, try again */
- baro_init();
- }
- }
- break;
-
- case MS5611_ADC_D1:
- /* read D1 (pressure) */
- ms5611_d1 = (ms5611_trans.input_buf[1] << 16) |
- (ms5611_trans.input_buf[2] << 8) |
- ms5611_trans.input_buf[3];
- /* start D2 conversion */
- ms5611_status = MS5611_CONV_D2;
- ms5611_trans.output_buf[0] = MS5611_START_CONV_D2;
- spi_submit(&(MS5611_SPI_DEV), &ms5611_trans);
- break;
-
- case MS5611_ADC_D2: {
- int64_t dt, baroms, tempms, off, sens, t2, off2, sens2;
- /* read D2 (temperature) */
- ms5611_d2 = (ms5611_trans.input_buf[1] << 16) |
- (ms5611_trans.input_buf[2] << 8) |
- ms5611_trans.input_buf[3];
- ms5611_status = MS5611_IDLE;
- ms5611_trans.status = SPITransDone;
-
- /* difference between actual and ref temperature */
- dt = ms5611_d2 - (int64_t)ms5611_c[5] * (1<<8);
- /* actual temperature */
- tempms = 2000 + ((int64_t)dt * ms5611_c[6]) / (1<<23);
- /* offset at actual temperature */
- off = ((int64_t)ms5611_c[2] * (1<<16)) + ((int64_t)ms5611_c[4] * dt) / (1<<7);
- /* sensitivity at actual temperature */
- sens = ((int64_t)ms5611_c[1] * (1<<15)) + ((int64_t)ms5611_c[3] * dt) / (1<<8);
- /* second order temperature compensation */
- if (tempms < 2000) {
- t2 = (dt*dt) / (1<<31);
- off2 = 5 * ((int64_t)(tempms-2000)*(tempms-2000)) / (1<<1);
- sens2 = 5 * ((int64_t)(tempms-2000)*(tempms-2000)) / (1<<2);
- if (tempms < -1500) {
- off2 = off2 + 7 * (int64_t)(tempms+1500)*(tempms+1500);
- sens2 = sens2 + 11 * ((int64_t)(tempms+1500)*(tempms+1500)) / (1<<1);
- }
- tempms = tempms - t2;
- off = off - off2;
- sens = sens - sens2;
- }
- /* temperature compensated pressure */
- baroms = (((int64_t)ms5611_d1 * sens) / (1<<21) - off) / (1<<15);
-
- /* Update baro structure */
- baro.absolute = (int32_t)baroms;
-
- b_abs_handler();
- b_diff_handler();
-
- #ifdef DEBUG
- ftempms = tempms / 100.;
- fbaroms = baroms / 100.;
- DOWNLINK_SEND_BARO_MS5611(DefaultChannel, DefaultDevice,
- &ms5611_d1, &ms5611_d2, &fbaroms, &ftempms, &baro.status);
- #endif
- break;
- }
-
- default:
- ms5611_trans.status = SPITransDone;
- break;
- }
- }
-}
diff --git a/sw/airborne/boards/lisa_s/baro_board.h b/sw/airborne/boards/lisa_s/baro_board.h
new file mode 100644
index 0000000000..7581ef4de3
--- /dev/null
+++ b/sw/airborne/boards/lisa_s/baro_board.h
@@ -0,0 +1,16 @@
+
+/*
+ * board specific functions for the lisa_m board
+ *
+ */
+
+#ifndef BOARDS_LISA_M_BARO_H
+#define BOARDS_LISA_M_BARO_H
+
+#include "std.h"
+
+extern void baro_event(void (*b_abs_handler)(void));
+
+#define BaroEvent(_b_abs_handler, _b_diff_handler) baro_event(_b_abs_handler)
+
+#endif /* BOARDS_LISA_M_BARO_H */
diff --git a/sw/airborne/boards/lisa_s_0.1.h b/sw/airborne/boards/lisa_s_0.1.h
new file mode 100644
index 0000000000..1182c1954e
--- /dev/null
+++ b/sw/airborne/boards/lisa_s_0.1.h
@@ -0,0 +1,429 @@
+#ifndef CONFIG_LISA_S_0_1_H
+#define CONFIG_LISA_S_0_1_H
+
+#define BOARD_LISA_S
+
+/* Lisa/M has a 12MHz external clock and 72MHz internal. */
+#define EXT_CLK 12000000
+#define AHB_CLK 72000000
+
+/*
+ * Onboard LEDs
+ */
+
+/* red, on PA8 */
+#ifndef USE_LED_1
+#define USE_LED_1 1
+#endif
+#define LED_1_GPIO GPIOC
+#define LED_1_GPIO_CLK RCC_APB2ENR_IOPCEN
+#define LED_1_GPIO_PIN GPIO10
+#define LED_1_GPIO_ON gpio_clear
+#define LED_1_GPIO_OFF gpio_set
+#define LED_1_AFIO_REMAP ((void)0)
+
+/* green, shared with JTAG_TRST */
+#ifndef USE_LED_2
+#define USE_LED_2 1
+#endif
+#define LED_2_GPIO GPIOC
+#define LED_2_GPIO_CLK RCC_APB2ENR_IOPCEN
+#define LED_2_GPIO_PIN GPIO11
+#define LED_2_GPIO_ON gpio_clear
+#define LED_2_GPIO_OFF gpio_set
+#define LED_2_AFIO_REMAP ((void)0)
+
+/* green, shared with ADC12 (ADC_6 on connector ANALOG2) */
+#ifndef USE_LED_3
+#define USE_LED_3 1
+#endif
+#define LED_3_GPIO GPIOD
+#define LED_3_GPIO_CLK RCC_APB2ENR_IOPDEN
+#define LED_3_GPIO_PIN GPIO2
+#define LED_3_GPIO_ON gpio_clear
+#define LED_3_GPIO_OFF gpio_set
+#define LED_3_AFIO_REMAP ((void)0)
+
+/*
+ * not actual LEDS, used as GPIOs
+ */
+
+/* PB1, DRDY on EXT SPI connector*/
+#define LED_BODY_GPIO GPIOB
+#define LED_BODY_GPIO_CLK RCC_APB2ENR_IOPBEN
+#define LED_BODY_GPIO_PIN GPIO1
+#define LED_BODY_GPIO_ON gpio_set
+#define LED_BODY_GPIO_OFF gpio_clear
+#define LED_BODY_AFIO_REMAP ((void)0)
+
+/* PC12, on GPIO connector*/
+#define LED_12_GPIO GPIOC
+#define LED_12_GPIO_CLK RCC_APB2ENR_IOPCEN
+#define LED_12_GPIO_PIN GPIO12
+#define LED_12_GPIO_ON gpio_clear
+#define LED_12_GPIO_OFF gpio_set
+#define LED_12_AFIO_REMAP ((void)0)
+
+
+/* Default actuators driver */
+#define DEFAULT_ACTUATORS "subsystems/actuators/actuators_pwm.h"
+#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y)
+#define ActuatorsDefaultInit() ActuatorsPwmInit()
+#define ActuatorsDefaultCommit() ActuatorsPwmCommit()
+
+
+#define DefaultVoltageOfAdc(adc) (0.0045*adc)
+
+/* Onboard ADCs */
+/*
+ ADC1 PC3/ADC13
+ ADC2 PC0/ADC10
+ ADC3 PC1/ADC11
+ ADC4 PC5/ADC15
+ ADC6 PC2/ADC12
+ BATT PC4/ADC14
+*/
+/* We should remove the first three adc channels, as Lisa/S does not provide those. */
+#define BOARD_ADC_CHANNEL_1 10
+#define BOARD_ADC_CHANNEL_2 11
+#define BOARD_ADC_CHANNEL_3 12
+// we can only use ADC1,2,3; the last channel is for bat monitoring
+#define BOARD_ADC_CHANNEL_4 2
+
+/* provide defines that can be used to access the ADC_x in the code or airframe file
+ * these directly map to the index number of the 4 adc channels defined above
+ * 4th (index 3) is used for bat monitoring by default
+ */
+#define ADC_1 0
+#define ADC_2 1
+#define ADC_3 2
+
+/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/
+#ifndef ADC_CHANNEL_VSUPPLY
+#define ADC_CHANNEL_VSUPPLY 3
+#endif
+
+/* GPIO mapping for ADC1 pins, overwrites the default in arch/stm32/mcu_periph/adc_arch.c */
+// FIXME, this is not very nice, is also locm3 lib specific
+#ifdef USE_AD1
+#define ADC1_GPIO_INIT(gpio) { \
+ gpio_set_mode(GPIOA, GPIO_MODE_INPUT, \
+ GPIO_CNF_INPUT_ANALOG, \
+ GPIO2); \
+ }
+#endif // USE_AD1
+
+#define BOARD_HAS_BARO 1
+
+/* SPI slave mapping */
+
+/* IMU_MPU_CS */
+#define SPI_SELECT_SLAVE0_PORT GPIOA
+#define SPI_SELECT_SLAVE0_PIN GPIO4
+
+/* IMU_BARO_CS */
+#define SPI_SELECT_SLAVE1_PORT GPIOC
+#define SPI_SELECT_SLAVE1_PIN GPIO4
+
+/* RADIO_SS */
+#define SPI_SELECT_SLAVE2_PORT GPIOB
+#define SPI_SELECT_SLAVE2_PIN GPIO12
+
+/*
+ * UART pin configuration
+ *
+ * sets on which pins the UARTs are connected
+ */
+/* DIAG port */
+#define UART1_GPIO_AF 0
+#define UART1_GPIO_PORT_RX GPIO_BANK_USART1_RX
+#define UART1_GPIO_RX GPIO_USART1_RX
+#define UART1_GPIO_PORT_TX GPIO_BANK_USART1_TX
+#define UART1_GPIO_TX GPIO_USART1_TX
+
+/* GPS */
+#define UART3_GPIO_AF 0
+#define UART3_GPIO_PORT_RX GPIO_BANK_USART3_RX
+#define UART3_GPIO_RX GPIO_USART3_RX
+#define UART3_GPIO_PORT_TX GPIO_BANK_USART3_TX
+#define UART3_GPIO_TX GPIO_USART3_TX
+
+/* LED1 & LED2 */
+/*
+#define UART3_GPIO_AF AFIO_MAPR_USART3_REMAP_PARTIAL_REMAP
+#define UART3_GPIO_PORT_RX GPIO_BANK_USART3_PR_RX
+#define UART3_GPIO_RX GPIO_USART3_PR_RX
+#define UART3_GPIO_PORT_TX GPIO_BANK_USART3_PR_TX
+#define UART3_GPIO_TX GPIO_USART3_PR_TX
+*/
+
+/*
+ * Spektrum
+ */
+/* The line that is pulled low at power up to initiate the bind process */
+#define SPEKTRUM_BIND_PIN GPIO2
+#define SPEKTRUM_BIND_PIN_PORT GPIOB
+
+/* Diag Port RX */
+#define SPEKTRUM_UART1_RCC_REG &RCC_APB2ENR
+#define SPEKTRUM_UART1_RCC_DEV RCC_APB2ENR_USART1EN
+#define SPEKTRUM_UART1_BANK GPIO_BANK_USART1_RX
+#define SPEKTRUM_UART1_PIN GPIO_USART1_RX
+#define SPEKTRUM_UART1_AF 0
+#define SPEKTRUM_UART1_IRQ NVIC_USART1_IRQ
+#define SPEKTRUM_UART1_ISR usart1_isr
+#define SPEKTRUM_UART1_DEV USART1
+
+/* AUX Radio RX */
+#define SPEKTRUM_UART2_RCC_REG &RCC_APB1ENR
+#define SPEKTRUM_UART2_RCC_DEV RCC_APB1ENR_USART2EN
+#define SPEKTRUM_UART2_BANK GPIO_BANK_USART2_RX
+#define SPEKTRUM_UART2_PIN GPIO_USART2_RX
+#define SPEKTRUM_UART2_AF 0
+#define SPEKTRUM_UART2_IRQ NVIC_USART2_IRQ
+#define SPEKTRUM_UART2_ISR usart2_isr
+#define SPEKTRUM_UART2_DEV USART2
+
+/* LED2 */
+#define SPEKTRUM_UART3_RCC_REG &RCC_APB1ENR
+#define SPEKTRUM_UART3_RCC_DEV RCC_APB1ENR_USART3EN
+#define SPEKTRUM_UART3_BANK GPIO_BANK_USART3_PR_RX
+#define SPEKTRUM_UART3_PIN GPIO_USART3_PR_RX
+#define SPEKTRUM_UART3_AF AFIO_MAPR_USART3_REMAP_PARTIAL_REMAP
+#define SPEKTRUM_UART3_IRQ NVIC_USART3_IRQ
+#define SPEKTRUM_UART3_ISR usart3_isr
+#define SPEKTRUM_UART3_DEV USART3
+
+/* LED3 */
+#define SPEKTRUM_UART5_RCC_REG &RCC_APB1ENR
+#define SPEKTRUM_UART5_RCC_DEV RCC_APB1ENR_UART5EN
+#define SPEKTRUM_UART5_BANK GPIO_BANK_UART5_RX
+#define SPEKTRUM_UART5_PIN GPIO_UART5_RX
+#define SPEKTRUM_UART5_AF 0
+#define SPEKTRUM_UART5_IRQ NVIC_UART5_IRQ
+#define SPEKTRUM_UART5_ISR uart5_isr
+#define SPEKTRUM_UART5_DEV UART5
+
+/* PPM
+ */
+
+/*
+ * On Lisa/S there is no really dedicated port available. But we could use a
+ * bunch of different pins to do PPM Input.
+ */
+
+/*
+ * Default is PPM config 2, input on GPIO01 (Servo pin 6)
+ */
+
+#ifndef PPM_CONFIG
+#define PPM_CONFIG 3
+#endif
+
+
+#if PPM_CONFIG == 1
+/* input on PA01 (UART1_RX) Diag port */
+#define USE_PPM_TIM1 1
+#define PPM_CHANNEL TIM_IC3
+#define PPM_TIMER_INPUT TIM_IC_IN_TI3
+#define PPM_IRQ NVIC_TIM1_UP_IRQ
+#define PPM_IRQ2 NVIC_TIM1_CC_IRQ
+// Capture/Compare InteruptEnable and InterruptFlag
+#define PPM_CC_IE TIM_DIER_CC3IE
+#define PPM_CC_IF TIM_SR_CC3IF
+#define PPM_GPIO_PORT GPIOA
+#define PPM_GPIO_PIN GPIO10
+#define PPM_GPIO_AF 0
+
+#elif PPM_CONFIG == 2
+/* input on PA1 (Servo 6 pin)
+ On Lisa/S we could also use TIM5 IC2 instead. */
+#define USE_PPM_TIM2 1
+#define PPM_CHANNEL TIM_IC2
+#define PPM_TIMER_INPUT TIM_IC_IN_TI2
+#define PPM_IRQ NVIC_TIM2_IRQ
+// Capture/Compare InteruptEnable and InterruptFlag
+#define PPM_CC_IE TIM_DIER_CC2IE
+#define PPM_CC_IF TIM_SR_CC2IF
+#define PPM_GPIO_PORT GPIOA
+#define PPM_GPIO_PIN GPIO1
+#define PPM_GPIO_AF 0
+
+// Move default ADC timer
+#if USE_AD_TIM2
+#undef USE_AD_TIM2
+#endif
+#define USE_AD_TIM1 1
+
+#elif PPM_CONFIG == 3
+/* input on PA3 (Aux RX pin)
+ On Lisa/S we could also use TIM5 IC4 instead. */
+#define USE_PPM_TIM2 1
+#define PPM_CHANNEL TIM_IC4
+#define PPM_TIMER_INPUT TIM_IC_IN_TI2
+#define PPM_IRQ NVIC_TIM2_IRQ
+// Capture/Compare InteruptEnable and InterruptFlag
+#define PPM_CC_IE TIM_DIER_CC4IE
+#define PPM_CC_IF TIM_SR_CC4IF
+#define PPM_GPIO_PORT GPIOA
+#define PPM_GPIO_PIN GPIO3
+#define PPM_GPIO_AF 0
+
+// Move default ADC timer
+#if USE_AD_TIM2
+#undef USE_AD_TIM2
+#endif
+#define USE_AD_TIM1 1
+
+#else
+#error "Unknown PPM config"
+
+#endif // PPM_CONFIG
+
+/* ADC */
+
+// active ADC
+#define USE_AD1 1
+#define USE_AD1_1 1
+#define USE_AD1_2 1
+#define USE_AD1_3 1
+#define USE_AD1_4 1
+
+/*
+ * I2C
+ *
+ */
+/* Servo1 & Servo2 */
+#define I2C1_GPIO_PORT GPIOB
+#define I2C1_GPIO_SCL GPIO6
+#define I2C1_GPIO_SDA GPIO7
+
+/* GPS TX & RX */
+#define I2C2_GPIO_PORT GPIOB
+#define I2C2_GPIO_SCL GPIO10
+#define I2C2_GPIO_SDA GPIO11
+
+/*
+ * PWM
+ *
+ */
+#define PWM_USE_TIM4 1
+#define PWM_USE_TIM5 1
+
+
+#if USE_SERVOS_1AND2
+ #if USE_I2C1
+ #error "You cannot USE_SERVOS_1AND2 and USE_I2C1 at the same time"
+ #else
+ #define ACTUATORS_PWM_NB 6
+ #define USE_PWM1 1
+ #define USE_PWM2 1
+ #define PWM_USE_TIM4 1
+ #endif
+#else
+ #define ACTUATORS_PWM_NB 4
+#endif
+
+#define USE_PWM3 1
+#define USE_PWM4 1
+#define USE_PWM5 1
+#define USE_PWM6 1
+
+// Servo numbering on LisaM silkscreen/docs starts with 1
+
+/* PWM_SERVO_x is the index of the servo in the actuators_pwm_values array */
+/* Because PWM1 and 2 can be disabled we put them at the index 4 & 5 so that it
+ * is easy to disable.
+ */
+#if USE_PWM1
+#define PWM_SERVO_1 4
+#define PWM_SERVO_1_TIMER TIM4
+#define PWM_SERVO_1_RCC_IOP RCC_APB2ENR_IOPBEN
+#define PWM_SERVO_1_GPIO GPIOB
+#define PWM_SERVO_1_PIN GPIO6
+#define PWM_SERVO_1_AF 0
+#define PWM_SERVO_1_OC TIM_OC1
+#define PWM_SERVO_1_OC_BIT (1<<0)
+#else
+#define PWM_SERVO_1_OC_BIT 0
+#endif
+
+#if USE_PWM2
+#define PWM_SERVO_2 5
+#define PWM_SERVO_2_TIMER TIM4
+#define PWM_SERVO_2_RCC_IOP RCC_APB2ENR_IOPBEN
+#define PWM_SERVO_2_GPIO GPIOB
+#define PWM_SERVO_2_PIN GPIO7
+#define PWM_SERVO_2_AF 0
+#define PWM_SERVO_2_OC TIM_OC2
+#define PWM_SERVO_2_OC_BIT (1<<1)
+#else
+#define PWM_SERVO_2_OC_BIT 0
+#endif
+
+#if USE_PWM3
+#define PWM_SERVO_3 0
+#define PWM_SERVO_3_TIMER TIM4
+#define PWM_SERVO_3_RCC_IOP RCC_APB2ENR_IOPBEN
+#define PWM_SERVO_3_GPIO GPIOB
+#define PWM_SERVO_3_PIN GPIO8
+#define PWM_SERVO_3_AF 0
+#define PWM_SERVO_3_OC TIM_OC3
+#define PWM_SERVO_3_OC_BIT (1<<2)
+#else
+#define PWM_SERVO_3_OC_BIT 0
+#endif
+
+#if USE_PWM4
+#define PWM_SERVO_4 1
+#define PWM_SERVO_4_TIMER TIM4
+#define PWM_SERVO_4_RCC_IOP RCC_APB2ENR_IOPBEN
+#define PWM_SERVO_4_GPIO GPIOB
+#define PWM_SERVO_4_PIN GPIO9
+#define PWM_SERVO_4_AF 0
+#define PWM_SERVO_4_OC TIM_OC4
+#define PWM_SERVO_4_OC_BIT (1<<3)
+#else
+#define PWM_SERVO_4_OC_BIT 0
+#endif
+
+#if USE_PWM5
+#define PWM_SERVO_5 2
+#define PWM_SERVO_5_TIMER TIM5
+#define PWM_SERVO_5_RCC_IOP RCC_APB2ENR_IOPAEN
+#define PWM_SERVO_5_GPIO GPIOA
+#define PWM_SERVO_5_PIN GPIO0
+#define PWM_SERVO_5_AF 0
+#define PWM_SERVO_5_OC TIM_OC1
+#define PWM_SERVO_5_OC_BIT (1<<0)
+#else
+#define PWM_SERVO_5_OC_BIT 0
+#endif
+
+#if USE_PWM6
+#define PWM_SERVO_6 3
+#define PWM_SERVO_6_TIMER TIM5
+#define PWM_SERVO_6_RCC_IOP RCC_APB2ENR_IOPAEN
+#define PWM_SERVO_6_GPIO GPIOA
+#define PWM_SERVO_6_PIN GPIO1
+#define PWM_SERVO_6_AF 0
+#define PWM_SERVO_6_OC TIM_OC2
+#define PWM_SERVO_6_OC_BIT (1<<1)
+#else
+#define PWM_SERVO_6_OC_BIT 0
+#endif
+
+/* servos 1-4 or 3-4 on TIM4 depending on USE_SERVOS_1AND2 */
+#define PWM_TIM4_CHAN_MASK (PWM_SERVO_1_OC_BIT|PWM_SERVO_2_OC_BIT|PWM_SERVO_3_OC_BIT|PWM_SERVO_4_OC_BIT)
+/* servos 5-6 on TIM5 */
+#define PWM_TIM5_CHAN_MASK (PWM_SERVO_5_OC_BIT|PWM_SERVO_6_OC_BIT)
+
+/* SuperbitRF mounted */
+#define SUPERBITRF_SPI_DEV spi2
+#define SUPERBITRF_RST_PORT GPIOC
+#define SUPERBITRF_RST_PIN GPIO9
+#define SUPERBITRF_DRDY_PORT GPIOC
+#define SUPERBITRF_DRDY_PIN GPIO6
+#define SUPERBITRF_FORCE_DSM2 FALSE
+
+#endif /* CONFIG_LISA_S_0_1_H */
diff --git a/sw/airborne/boards/px4fmu/baro_board.h b/sw/airborne/boards/px4fmu/baro_board.h
new file mode 100644
index 0000000000..4aff1cc1c7
--- /dev/null
+++ b/sw/airborne/boards/px4fmu/baro_board.h
@@ -0,0 +1,14 @@
+
+/*
+ * board specific fonctions for the PX4FMU board
+ *
+ */
+
+#ifndef BOARDS_PX4FMU_BARO_H
+#define BOARDS_PX4FMU_BARO_H
+
+extern void baro_event(void (*b_abs_handler)(void));
+
+#define BaroEvent(_b_abs_handler, _b_diff_handler) baro_event(_b_abs_handler)
+
+#endif /* BOARDS_PX4FMU_BARO_H */
diff --git a/sw/airborne/boards/px4fmu_1.7.h b/sw/airborne/boards/px4fmu_1.7.h
new file mode 100644
index 0000000000..deb299a1d6
--- /dev/null
+++ b/sw/airborne/boards/px4fmu_1.7.h
@@ -0,0 +1,313 @@
+#ifndef CONFIG_PX4FMU_1_7_H
+#define CONFIG_PX4FMU_1_7_H
+
+#define BOARD_PX4FMU
+
+/* PX4 has a 16MHz external clock and 168MHz internal. */
+#define EXT_CLK 16000000
+#define AHB_CLK 168000000
+
+
+/*
+ * Onboard LEDs
+ */
+/* red/amber , on PB14 */
+#ifndef USE_LED_1
+#define USE_LED_1 1
+#endif
+#define LED_1_GPIO GPIOB
+#define LED_1_GPIO_CLK RCC_AHB1ENR_IOPBEN
+#define LED_1_GPIO_PIN GPIO14
+#define LED_1_GPIO_ON gpio_clear
+#define LED_1_GPIO_OFF gpio_set
+#define LED_1_AFIO_REMAP ((void)0)
+
+/* blue, on PB15 */
+#ifndef USE_LED_2
+#define USE_LED_2 1
+#endif
+#define LED_2_GPIO GPIOB
+#define LED_2_GPIO_CLK RCC_AHB1ENR_IOPBEN
+#define LED_2_GPIO_PIN GPIO15
+#define LED_2_GPIO_ON gpio_clear
+#define LED_2_GPIO_OFF gpio_set
+#define LED_2_AFIO_REMAP ((void)0)
+
+
+
+/*
+ * UART
+ */
+#define UART1_GPIO_AF GPIO_AF7
+#define UART1_GPIO_PORT_RX GPIOB
+#define UART1_GPIO_RX GPIO7
+#define UART1_GPIO_PORT_TX GPIOB
+#define UART1_GPIO_TX GPIO6
+
+#define UART2_GPIO_AF GPIO_AF7
+#define UART2_GPIO_PORT_RX GPIOA
+#define UART2_GPIO_RX GPIO3
+#define UART2_GPIO_PORT_TX GPIOA
+#define UART2_GPIO_TX GPIO2
+
+#define UART6_GPIO_AF GPIO_AF8
+#define UART6_GPIO_PORT_RX GPIOC
+#define UART6_GPIO_RX GPIO7
+#define UART6_GPIO_PORT_TX GPIOC
+#define UART6_GPIO_TX GPIO6
+
+
+/*
+ * SPI
+ */
+/* SPI1 for MPU and accel/gyro if populated */
+#define SPI1_GPIO_AF GPIO_AF5
+#define SPI1_GPIO_PORT_MISO GPIOA
+#define SPI1_GPIO_MISO GPIO6
+#define SPI1_GPIO_PORT_MOSI GPIOA
+#define SPI1_GPIO_MOSI GPIO7
+#define SPI1_GPIO_PORT_SCK GPIOA
+#define SPI1_GPIO_SCK GPIO5
+
+/* SPI3 on microSD connector */
+#define SPI3_GPIO_AF GPIO_AF5
+#define SPI3_GPIO_PORT_MISO GPIOC
+#define SPI3_GPIO_MISO GPIO11
+#define SPI3_GPIO_PORT_MOSI GPIOB
+#define SPI3_GPIO_MOSI GPIO5
+#define SPI3_GPIO_PORT_SCK GPIOC
+#define SPI3_GPIO_SCK GPIO10
+
+/*
+ * SPI slave pin declaration
+ */
+/* GYRO_CS on SPI1 */
+#define SPI_SELECT_SLAVE0_PORT GPIOC
+#define SPI_SELECT_SLAVE0_PIN GPIO14
+
+/* ACCEL_CS on SPI1 */
+#define SPI_SELECT_SLAVE1_PORT GPIOC
+#define SPI_SELECT_SLAVE1_PIN GPIO15
+
+/* MPU_CS on SPI1 */
+#define SPI_SELECT_SLAVE2_PORT GPIOB
+#define SPI_SELECT_SLAVE2_PIN GPIO0
+
+/* SPI3 NSS on microSD connector */
+#define SPI_SELECT_SLAVE3_PORT GPIOA
+#define SPI_SELECT_SLAVE3_PIN GPIO4
+
+
+/* Onboard ADCs */
+#define USE_AD_TIM4 1
+
+#define BOARD_ADC_CHANNEL_1 11
+#define BOARD_ADC_CHANNEL_2 12
+#define BOARD_ADC_CHANNEL_3 13
+#define BOARD_ADC_CHANNEL_4 10
+
+#ifndef USE_AD1
+#define USE_AD1 1
+#endif
+/* provide defines that can be used to access the ADC_x in the code or airframe file
+ * these directly map to the index number of the 4 adc channels defined above
+ * 4th (index 3) is used for bat monitoring by default
+ */
+#define ADC_1 ADC1_C1
+#ifdef USE_ADC_1
+#ifndef ADC_1_GPIO_CLOCK_PORT
+#define ADC_1_GPIO_CLOCK_PORT RCC_AHB1ENR_IOPCEN
+#define ADC_1_INIT() gpio_mode_setup(GPIOC, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO1)
+#endif
+#define USE_AD1_1 1
+#else
+#define ADC_1_GPIO_CLOCK_PORT 0
+#define ADC_1_INIT() {}
+#endif
+
+#define ADC_2 ADC1_C2
+#ifdef USE_ADC_2
+#ifndef ADC_2_GPIO_CLOCK_PORT
+#define ADC_2_GPIO_CLOCK_PORT RCC_AHB1ENR_IOPCEN
+#define ADC_2_INIT() gpio_mode_setup(GPIOC, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO2)
+#endif
+#define USE_AD1_2 1
+#else
+#define ADC_2_GPIO_CLOCK_PORT 0
+#define ADC_2_INIT() {}
+#endif
+
+#define ADC_3 ADC1_C3
+#ifdef USE_ADC_3
+#ifndef ADC_3_GPIO_CLOCK_PORT
+#define ADC_3_GPIO_CLOCK_PORT RCC_AHB1ENR_IOPCEN
+#define ADC_3_INIT() gpio_mode_setup(GPIOC, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO3)
+#endif
+#define USE_AD1_3 1
+#else
+#define ADC_3_GPIO_CLOCK_PORT 0
+#define ADC_3_INIT() {}
+#endif
+
+#define ADC_4 ADC1_C4
+#ifndef ADC_4_GPIO_CLOCK_PORT
+#define ADC_4_GPIO_CLOCK_PORT RCC_AHB1ENR_IOPCEN
+#define ADC_4_INIT() gpio_mode_setup(GPIOC, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO0)
+#endif
+#define USE_AD1_4 1
+
+/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/
+#ifndef ADC_CHANNEL_VSUPPLY
+#define ADC_CHANNEL_VSUPPLY ADC_4
+#endif
+#define DefaultVoltageOfAdc(adc) (0.006185*adc)
+
+#define ADC_GPIO_CLOCK_PORT (ADC_1_GPIO_CLOCK_PORT | ADC_2_GPIO_CLOCK_PORT | ADC_3_GPIO_CLOCK_PORT | ADC_4_GPIO_CLOCK_PORT)
+
+/* GPIO mapping for ADC1 pins, overwrites the default in arch/stm32/mcu_periph/adc_arch.c */
+#ifdef USE_AD1
+#define ADC1_GPIO_INIT(gpio) { \
+ ADC_1_INIT(); \
+ ADC_2_INIT(); \
+ ADC_3_INIT(); \
+ ADC_4_INIT(); \
+}
+#endif // USE_AD1
+
+
+/*
+ * I2C mapping
+ */
+#define I2C1_GPIO_PORT GPIOB
+#define I2C1_GPIO_SCL GPIO8
+#define I2C1_GPIO_SDA GPIO9
+
+#define I2C2_GPIO_PORT GPIOB
+#define I2C2_GPIO_SCL GPIO10
+#define I2C2_GPIO_SDA GPIO11
+
+#define I2C3_GPIO_PORT_SCL GPIOA
+#define I2C3_GPIO_SCL GPIO8
+#define I2C3_GPIO_PORT_SDA GPIOC
+#define I2C3_GPIO_SDA GPIO9
+
+
+/*
+ * PPM
+ */
+#define USE_PPM_TIM1 1
+
+#define PPM_CHANNEL TIM_IC1
+#define PPM_TIMER_INPUT TIM_IC_IN_TI1
+#define PPM_IRQ NVIC_TIM1_CC_IRQ
+#define PPM_IRQ2 NVIC_TIM1_UP_TIM10_IRQ
+// Capture/Compare InteruptEnable and InterruptFlag
+#define PPM_CC_IE TIM_DIER_CC1IE
+#define PPM_CC_IF TIM_SR_CC1IF
+#define PPM_GPIO_PORT GPIOA
+#define PPM_GPIO_PIN GPIO10
+#define PPM_GPIO_AF GPIO_AF1
+
+
+/*
+ * Spektrum
+ */
+/* The line that is pulled low at power up to initiate the bind process */
+/* GPIO_EXT1 on PX4FMU */
+#define SPEKTRUM_BIND_PIN GPIO4
+#define SPEKTRUM_BIND_PIN_PORT GPIOC
+
+#define SPEKTRUM_UART2_RCC_REG &RCC_APB1ENR
+#define SPEKTRUM_UART2_RCC_DEV RCC_APB1ENR_USART2EN
+#define SPEKTRUM_UART2_BANK GPIOA
+#define SPEKTRUM_UART2_PIN GPIO3
+#define SPEKTRUM_UART2_AF GPIO_AF7
+#define SPEKTRUM_UART2_IRQ NVIC_USART2_IRQ
+#define SPEKTRUM_UART2_ISR usart2_isr
+#define SPEKTRUM_UART2_DEV USART2
+
+
+
+/* Activate onboard baro */
+#define BOARD_HAS_BARO 1
+
+
+
+/* Default actuators driver */
+#define DEFAULT_ACTUATORS "subsystems/actuators/actuators_pwm.h"
+#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y)
+#define ActuatorsDefaultInit() ActuatorsPwmInit()
+#define ActuatorsDefaultCommit() ActuatorsPwmCommit()
+
+/* PWM */
+#define PWM_USE_TIM2 1
+
+#define USE_PWM1 1
+#define USE_PWM2 1
+#define USE_PWM3 1
+#define USE_PWM4 1
+
+// Servo numbering on the PX4 starts with 1
+
+// PWM_SERVO_x is the index of the servo in the actuators_pwm_values array
+// SRV1 is also UART2_CTS
+#if USE_PWM1
+#define PWM_SERVO_1 0
+#define PWM_SERVO_1_TIMER TIM2
+#define PWM_SERVO_1_RCC_IOP RCC_AHB1ENR_IOPAEN
+#define PWM_SERVO_1_GPIO GPIOA
+#define PWM_SERVO_1_PIN GPIO0
+#define PWM_SERVO_1_AF GPIO_AF1
+#define PWM_SERVO_1_OC TIM_OC1
+#define PWM_SERVO_1_OC_BIT (1<<0)
+#else
+#define PWM_SERVO_1_OC_BIT 0
+#endif
+
+// SRV2 is also UART2_RTS
+#if USE_PWM2
+#define PWM_SERVO_2 1
+#define PWM_SERVO_2_TIMER TIM2
+#define PWM_SERVO_2_RCC_IOP RCC_AHB1ENR_IOPAEN
+#define PWM_SERVO_2_GPIO GPIOA
+#define PWM_SERVO_2_PIN GPIO1
+#define PWM_SERVO_2_AF GPIO_AF1
+#define PWM_SERVO_2_OC TIM_OC2
+#define PWM_SERVO_2_OC_BIT (1<<1)
+#else
+#define PWM_SERVO_2_OC_BIT 0
+#endif
+
+// SRV3 is also UART2_TX
+#if USE_PWM3
+#define PWM_SERVO_3_IDX 2
+#define PWM_SERVO_3_TIMER TIM2
+#define PWM_SERVO_3_RCC_IOP RCC_AHB1ENR_IOPAEN
+#define PWM_SERVO_3_GPIO GPIOA
+#define PWM_SERVO_3_PIN GPIO2
+#define PWM_SERVO_3_AF GPIO_AF1
+#define PWM_SERVO_3_OC TIM_OC3
+#define PWM_SERVO_3_OC_BIT (1<<2)
+#else
+#define PWM_SERVO_3_OC_BIT 0
+#endif
+
+// SRV4 is also UART2_RX
+#if USE_PWM4
+#define PWM_SERVO_4 3
+#define PWM_SERVO_4_TIMER TIM2
+#define PWM_SERVO_4_RCC_IOP RCC_AHB1ENR_IOPAEN
+#define PWM_SERVO_4_GPIO GPIOA
+#define PWM_SERVO_4_PIN GPIO3
+#define PWM_SERVO_4_AF GPIO_AF1
+#define PWM_SERVO_4_OC TIM_OC4
+#define PWM_SERVO_4_OC_BIT (1<<3)
+#else
+#define PWM_SERVO_4_OC_BIT 0
+#endif
+
+
+#define PWM_TIM2_CHAN_MASK (PWM_SERVO_1_OC_BIT|PWM_SERVO_2_OC_BIT|PWM_SERVO_3_OC_BIT|PWM_SERVO_4_OC_BIT)
+
+
+#endif /* CONFIG_PX4FMU_1_7_H */
diff --git a/sw/airborne/boards/stm32f4_discovery.h b/sw/airborne/boards/stm32f4_discovery.h
new file mode 100755
index 0000000000..288385d132
--- /dev/null
+++ b/sw/airborne/boards/stm32f4_discovery.h
@@ -0,0 +1,236 @@
+#ifndef CONFIG_STM32F4_DISCOVERY_H
+#define CONFIG_STM32F4_DISCOVERY_H
+
+#define BOARD_STM32F4_DISCOVERY
+
+/* STM32F4_DISCOVERY has a 8MHz external clock and 168MHz internal. */
+#define EXT_CLK 8000000
+#define AHB_CLK 168000000
+
+/*
+ * Onboard LEDs
+ */
+
+/* orange, on PD13 */
+#ifndef USE_LED_3
+#define USE_LED_3 1
+#endif
+#define LED_3_GPIO GPIOD
+#define LED_3_GPIO_CLK RCC_AHB1ENR_IOPDEN
+#define LED_3_GPIO_PIN GPIO13
+#define LED_3_AFIO_REMAP ((void)0)
+#define LED_3_GPIO_ON gpio_set
+#define LED_3_GPIO_OFF gpio_clear
+
+/* green, on PD12 */
+#ifndef USE_LED_4
+#define USE_LED_4 1
+#endif
+#define LED_4_GPIO GPIOD
+#define LED_4_GPIO_CLK RCC_AHB1ENR_IOPDEN
+#define LED_4_GPIO_PIN GPIO12
+#define LED_4_AFIO_REMAP ((void)0)
+#define LED_4_GPIO_ON gpio_set
+#define LED_4_GPIO_OFF gpio_clear
+
+/* red, PD14 */
+#ifndef USE_LED_5
+#define USE_LED_5 1
+#endif
+#define LED_5_GPIO GPIOD
+#define LED_5_GPIO_CLK RCC_AHB1ENR_IOPDEN
+#define LED_5_GPIO_PIN GPIO14
+#define LED_5_AFIO_REMAP ((void)0)
+#define LED_5_GPIO_ON gpio_set
+#define LED_5_GPIO_OFF gpio_clear
+
+/* blue, PD15 */
+#ifndef USE_LED_6
+#define USE_LED_6 1
+#endif
+#define LED_6_GPIO GPIOD
+#define LED_6_GPIO_CLK RCC_AHB1ENR_IOPDEN
+#define LED_6_GPIO_PIN GPIO15
+#define LED_6_AFIO_REMAP ((void)0)
+#define LED_6_GPIO_ON gpio_set
+#define LED_6_GPIO_OFF gpio_clear
+
+
+/* UART */
+#define UART1_GPIO_AF GPIO_AF7
+#define UART1_GPIO_PORT_RX GPIOA
+#define UART1_GPIO_RX GPIO10
+#define UART1_GPIO_PORT_TX GPIOB
+#define UART1_GPIO_TX GPIO6
+
+#define UART2_GPIO_AF GPIO_AF7
+#define UART2_GPIO_PORT_RX GPIOA
+#define UART2_GPIO_RX GPIO3
+#define UART2_GPIO_PORT_TX GPIOA
+#define UART2_GPIO_TX GPIO2
+
+#define UART4_GPIO_AF GPIO_AF8
+#define UART4_GPIO_PORT_RX GPIOA
+#define UART4_GPIO_RX GPIO1
+#define UART4_GPIO_PORT_TX GPIOA
+#define UART4_GPIO_TX GPIO0
+
+#define UART6_GPIO_AF GPIO_AF8
+#define UART6_GPIO_PORT_RX GPIOC
+#define UART6_GPIO_RX GPIO7
+#define UART6_GPIO_PORT_TX GPIOC
+#define UART6_GPIO_TX GPIO6
+
+
+/* SPI */
+#define SPI1_GPIO_AF GPIO_AF5
+#define SPI1_GPIO_PORT_MISO GPIOA
+#define SPI1_GPIO_MISO GPIO6
+#define SPI1_GPIO_PORT_MOSI GPIOA
+#define SPI1_GPIO_MOSI GPIO7
+#define SPI1_GPIO_PORT_SCK GPIOA
+#define SPI1_GPIO_SCK GPIO5
+
+#define SPI_SELECT_SLAVE0_PORT GPIOB
+#define SPI_SELECT_SLAVE0_PIN GPIO9
+
+
+/* I2C mapping */
+#define I2C1_GPIO_PORT GPIOB
+#define I2C1_GPIO_SCL GPIO8
+#define I2C1_GPIO_SDA GPIO9
+
+#define I2C2_GPIO_PORT GPIOB
+#define I2C2_GPIO_SCL GPIO10
+#define I2C2_GPIO_SDA GPIO11
+
+#define I2C3_GPIO_PORT_SCL GPIOA
+#define I2C3_GPIO_PORT_SDA GPIOC
+#define I2C3_GPIO_SCL GPIO8
+#define I2C3_GPIO_SDA GPIO9
+
+
+/*
+ * PPM
+ */
+#define USE_PPM_TIM1 1
+
+#define PPM_CHANNEL TIM_IC1
+#define PPM_TIMER_INPUT TIM_IC_IN_TI1
+#define PPM_IRQ NVIC_TIM1_CC_IRQ
+#define PPM_IRQ2 NVIC_TIM1_UP_TIM10_IRQ
+// Capture/Compare InteruptEnable and InterruptFlag
+#define PPM_CC_IE TIM_DIER_CC1IE
+#define PPM_CC_IF TIM_SR_CC1IF
+#define PPM_GPIO_PORT GPIOA
+#define PPM_GPIO_PIN GPIO8
+#define PPM_GPIO_AF GPIO_AF1
+
+
+/*
+ * Spektrum
+ */
+/* The line that is pulled low at power up to initiate the bind process */
+#define SPEKTRUM_BIND_PIN GPIO8
+#define SPEKTRUM_BIND_PIN_PORT GPIOA
+
+#define SPEKTRUM_UART2_RCC_REG &RCC_APB2ENR
+#define SPEKTRUM_UART2_RCC_DEV RCC_APB2ENR_USART1EN
+#define SPEKTRUM_UART2_BANK GPIOA
+#define SPEKTRUM_UART2_PIN GPIO10
+#define SPEKTRUM_UART2_AF GPIO_AF7
+#define SPEKTRUM_UART2_IRQ NVIC_USART1_IRQ
+#define SPEKTRUM_UART2_ISR usart1_isr
+#define SPEKTRUM_UART2_DEV USART1
+
+
+/* Onboard ADCs */
+#define USE_AD_TIM4 1
+
+#define BOARD_ADC_CHANNEL_1 9
+#define BOARD_ADC_CHANNEL_2 15
+#define BOARD_ADC_CHANNEL_3 14
+#define BOARD_ADC_CHANNEL_4 4
+
+#ifndef USE_AD1
+#define USE_AD1 1
+#endif
+/* provide defines that can be used to access the ADC_x in the code or airframe file
+ * these directly map to the index number of the 4 adc channels defined above
+ * 4th (index 3) is used for bat monitoring by default
+ */
+// AUX 1
+#define ADC_1 ADC1_C1
+#ifdef USE_ADC_1
+#ifndef ADC_1_GPIO_CLOCK_PORT
+#define ADC_1_GPIO_CLOCK_PORT RCC_AHB1ENR_IOPBEN
+#define ADC_1_INIT() gpio_mode_setup(GPIOB, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO1)
+#endif
+#define USE_AD1_1 1
+#else
+#define ADC_1_GPIO_CLOCK_PORT 0
+#define ADC_1_INIT() {}
+#endif
+
+// AUX 2
+#define ADC_2 ADC1_C2
+#ifdef USE_ADC_2
+#ifndef ADC_2_GPIO_CLOCK_PORT
+#define ADC_2_GPIO_CLOCK_PORT RCC_AHB1ENR_IOPCEN
+#define ADC_2_INIT() gpio_mode_setup(GPIOC, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO5)
+#endif
+#define USE_AD1_2 1
+#else
+#define ADC_2_GPIO_CLOCK_PORT 0
+#define ADC_2_INIT() {}
+#endif
+
+// AUX 3
+#define ADC_3 ADC1_C3
+#ifdef USE_ADC_3
+#ifndef ADC_3_GPIO_CLOCK_PORT
+#define ADC_3_GPIO_CLOCK_PORT RCC_AHB1ENR_IOPCEN
+#define ADC_3_INIT() gpio_mode_setup(GPIOC, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO4)
+#endif
+#define USE_AD1_3 1
+#else
+#define ADC_3_GPIO_CLOCK_PORT 0
+#define ADC_3_INIT() {}
+#endif
+
+// BAT
+#define ADC_4 ADC1_C4
+#ifndef ADC_4_GPIO_CLOCK_PORT
+#define ADC_4_GPIO_CLOCK_PORT RCC_AHB1ENR_IOPAEN
+#define ADC_4_INIT() gpio_mode_setup(GPIOA, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO4)
+#endif
+#define USE_AD1_4 1
+
+/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/
+#ifndef ADC_CHANNEL_VSUPPLY
+#define ADC_CHANNEL_VSUPPLY ADC_4
+#endif
+
+#define ADC_GPIO_CLOCK_PORT (ADC_1_GPIO_CLOCK_PORT | ADC_2_GPIO_CLOCK_PORT | ADC_3_GPIO_CLOCK_PORT | ADC_4_GPIO_CLOCK_PORT)
+
+/* GPIO mapping for ADC1 pins, overwrites the default in arch/stm32/mcu_periph/adc_arch.c */
+#ifdef USE_AD1
+#define ADC1_GPIO_INIT(gpio) { \
+ ADC_1_INIT(); \
+ ADC_2_INIT(); \
+ ADC_3_INIT(); \
+ ADC_4_INIT(); \
+ }
+#endif // USE_AD1
+
+#define DefaultVoltageOfAdc(adc) (0.00485*adc)
+
+
+/* Default actuators driver */
+#define DEFAULT_ACTUATORS "subsystems/actuators/actuators_pwm.h"
+#define ActuatorDefaultSet(_x,_y) ActuatorPwmSet(_x,_y)
+#define ActuatorsDefaultInit() ActuatorsPwmInit()
+#define ActuatorsDefaultCommit() ActuatorsPwmCommit()
+
+
+#endif /* CONFIG_STM32F4_DISCOVERY_H */
diff --git a/sw/airborne/firmwares/beth/overo_test_uart.c b/sw/airborne/firmwares/beth/overo_test_uart.c
index add7fb9055..572696f762 100644
--- a/sw/airborne/firmwares/beth/overo_test_uart.c
+++ b/sw/airborne/firmwares/beth/overo_test_uart.c
@@ -37,7 +37,7 @@
#include "fms_periodic.h"
#include "fms_debug.h"
-#include "fms_serial_port.h"
+#include "serial_port.h"
#include "overo_gcs_com.h"
#include "uart_hw.h"
@@ -57,7 +57,7 @@ void check_gps(void);
uint8_t nav_utm_zone0 = 31;
static uint16_t foo = 0;
-//struct FmsSerialPort* fmssp;
+//struct SerialPort* fmssp;
//int spfd;
uint8_t portnum;
#ifdef GPS_CONFIGURE
diff --git a/sw/airborne/firmwares/beth/uart_hw.c b/sw/airborne/firmwares/beth/uart_hw.c
index 558747b871..2d05231221 100644
--- a/sw/airborne/firmwares/beth/uart_hw.c
+++ b/sw/airborne/firmwares/beth/uart_hw.c
@@ -27,7 +27,7 @@
#include
#include
-#include "fms_serial_port.h"
+#include "serial_port.h"
#ifdef USE_UART0
@@ -39,7 +39,7 @@ volatile uint16_t uart0_tx_insert_idx, uart0_tx_extract_idx;
volatile bool_t uart0_tx_running;
uint8_t uart0_tx_buffer[UART0_TX_BUFFER_SIZE];
-struct FmsSerialPort* fmssp0;
+struct SerialPort* fmssp0;
int uart0_fd;
extern uint8_t portnum;
@@ -165,7 +165,7 @@ volatile uint16_t uart1_tx_insert_idx, uart1_tx_extract_idx;
volatile bool_t uart1_tx_running;
uint8_t uart1_tx_buffer[UART1_TX_BUFFER_SIZE];
-struct FmsSerialPort* fmssp1;
+struct SerialPort* fmssp1;
int uart1_fd;
void uart1_init( void ) {
diff --git a/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c
index 3f07c7b0ac..6ec32bf2da 100644
--- a/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c
+++ b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c
@@ -95,7 +95,7 @@ float v_ctl_airspeed_pgain;
float v_ctl_altitude_error; ///< in meters, (setpoint - alt) -> positive = too low
float v_ctl_max_climb;
-float v_ctl_max_acceleration = 0.5;
+float v_ctl_max_acceleration;
/* inner loop */
float v_ctl_climb_setpoint;
@@ -145,10 +145,16 @@ float v_ctl_pitch_setpoint;
#warning "No STALL_AIRSPEED defined. Using NOMINAL_AIRSPEED"
#define STALL_AIRSPEED NOMINAL_AIRSPEED
#endif
+#ifndef V_CTL_GLIDE_RATIO
+#define V_CTL_GLIDE_RATIO 8.
+#warning "V_CTL_GLIDE_RATIO not defined - default is 8."
+#endif
#ifndef AIRSPEED_SETPOINT_SLEW
#define AIRSPEED_SETPOINT_SLEW 1
#endif
-
+#ifndef V_CTL_MAX_ACCELERATION
+#define V_CTL_MAX_ACCELERATION 0.5
+#endif
/////////////////////////////////////////////////
// Automatically found airplane characteristics
@@ -162,9 +168,9 @@ float ac_char_cruise_throttle = 0.0f;
float ac_char_cruise_pitch = 0.0f;
int ac_char_cruise_count = 0;
-static void ac_char_average(float* last, float new, int count)
+static void ac_char_average(float* last_v, float new_v, int count)
{
- *last = (((*last) * (((float)count) - 1.0f)) + new) / ((float) count);
+ *last_v = (((*last_v) * (((float)count) - 1.0f)) + new_v) / ((float) count);
}
static void ac_char_update(float throttle, float pitch, float climb, float accelerate)
@@ -212,6 +218,8 @@ void v_ctl_init( void ) {
v_ctl_auto_airspeed_setpoint_slew = v_ctl_auto_airspeed_setpoint;
v_ctl_airspeed_pgain = V_CTL_AIRSPEED_PGAIN;
+ v_ctl_max_acceleration = V_CTL_MAX_ACCELERATION;
+
/* inner loops */
v_ctl_climb_setpoint = 0.;
@@ -345,7 +353,7 @@ void v_ctl_climb_loop( void )
float vdot_err = low_pass_vdot( v_ctl_desired_acceleration - vdot );
// Flight Path Outerloop: positive means needs to climb more: needs extra energy
- float gamma_err = (v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z) / v_ctl_auto_airspeed_setpoint;
+ float gamma_err = (v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z) / v_ctl_auto_airspeed_controlled;
// Total Energy Error: positive means energy should be added
float en_tot_err = gamma_err + vdot_err;
@@ -392,6 +400,7 @@ void v_ctl_climb_loop( void )
+ v_ctl_auto_pitch_of_airspeed_dgain * vdot
+ v_ctl_energy_diff_pgain * en_dis_err
+ v_ctl_auto_throttle_nominal_cruise_pitch;
+if(kill_throttle) v_ctl_pitch_of_vz = v_ctl_pitch_of_vz - 1/V_CTL_GLIDE_RATIO;
v_ctl_pitch_setpoint = v_ctl_pitch_of_vz + nav_pitch;
Bound(v_ctl_pitch_setpoint,H_CTL_PITCH_MIN_SETPOINT,H_CTL_PITCH_MAX_SETPOINT)
diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
index 4455ca3641..859b3bda08 100644
--- a/sw/airborne/firmwares/fixedwing/main_ap.c
+++ b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -84,19 +84,36 @@
#include "led.h"
+#ifdef USE_NPS
+#include "nps_autopilot_fixedwing.h"
+#endif
+
+/* Default trim commands for roll, pitch and yaw */
+#ifndef COMMAND_ROLL_TRIM
+#define COMMAND_ROLL_TRIM 0
+#endif
+
+#ifndef COMMAND_PITCH_TRIM
+#define COMMAND_PITCH_TRIM 0
+#endif
+
+#ifndef COMMAND_YAW_TRIM
+#define COMMAND_YAW_TRIM 0
+#endif
+
/* if PRINT_CONFIG is defined, print some config options */
PRINT_CONFIG_VAR(PERIODIC_FREQUENCY)
PRINT_CONFIG_VAR(NAVIGATION_FREQUENCY)
PRINT_CONFIG_VAR(CONTROL_FREQUENCY)
-#ifndef TELEMETRY_FREQUENCY
-#define TELEMETRY_FREQUENCY 60
-#endif
+/* TELEMETRY_FREQUENCY is defined in generated/periodic_telemetry.h
+ * defaults to 60Hz or set by TELEMETRY_FREQUENCY configure option in airframe file
+ */
PRINT_CONFIG_VAR(TELEMETRY_FREQUENCY)
-#ifndef MODULES_FREQUENCY
-#define MODULES_FREQUENCY 60
-#endif
+/* MODULES_FREQUENCY is defined in generated/modules.h
+ * according to main_freq parameter set for modules in airframe file
+ */
PRINT_CONFIG_VAR(MODULES_FREQUENCY)
@@ -168,6 +185,9 @@ void init_ap( void ) {
#if USE_IMU
imu_init();
+#if USE_IMU_FLOAT
+ imu_float_init();
+#endif
#endif
#if USE_AHRS_ALIGNER
@@ -237,6 +257,12 @@ void init_ap( void ) {
traffic_info_init();
#endif
+ /* set initial trim values.
+ * these are passed to fbw via inter_mcu.
+ */
+ ap_state->command_roll_trim = COMMAND_ROLL_TRIM;
+ ap_state->command_pitch_trim = COMMAND_PITCH_TRIM;
+ ap_state->command_yaw_trim = COMMAND_YAW_TRIM;
}
@@ -563,7 +589,7 @@ void sensors_task( void ) {
#endif // USE_IMU
//FIXME: this is just a kludge
-#if USE_AHRS && defined SITL
+#if USE_AHRS && defined SITL && !USE_NPS
ahrs_propagate();
#endif
@@ -660,7 +686,6 @@ void event_task_ap( void ) {
if (new_ins_attitude > 0)
{
attitude_loop();
- //LED_TOGGLE(3);
new_ins_attitude = 0;
}
#endif
@@ -709,10 +734,6 @@ static inline void on_gyro_event( void ) {
ahrs_propagate();
ahrs_update_accel();
-#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
- new_ins_attitude = 1;
-#endif
-
#else //PERIODIC_FREQUENCY
static uint8_t _reduced_propagation_rate = 0;
static uint8_t _reduced_correction_rate = 0;
@@ -725,6 +746,7 @@ static inline void on_gyro_event( void ) {
_reduced_propagation_rate++;
if (_reduced_propagation_rate < (PERIODIC_FREQUENCY / AHRS_PROPAGATE_FREQUENCY))
{
+ return;
}
else
{
@@ -746,13 +768,17 @@ static inline void on_gyro_event( void ) {
ImuScaleAccel(imu);
ahrs_update_accel();
}
-
-#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
- new_ins_attitude = 1;
-#endif
}
#endif //PERIODIC_FREQUENCY
+#if defined SITL && USE_NPS
+ if (nps_bypass_ahrs) sim_overwrite_ahrs();
+#endif
+
+#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
+ new_ins_attitude = 1;
+#endif
+
}
static inline void on_mag_event(void)
diff --git a/sw/airborne/firmwares/fixedwing/main_fbw.c b/sw/airborne/firmwares/fixedwing/main_fbw.c
index 6b0334ab12..2e1e1f865d 100644
--- a/sw/airborne/firmwares/fixedwing/main_fbw.c
+++ b/sw/airborne/firmwares/fixedwing/main_fbw.c
@@ -60,17 +60,13 @@ uint8_t fbw_mode;
#include "inter_mcu.h"
+#ifdef USE_NPS
+#include "nps_autopilot_fixedwing.h"
+#endif
-/** Trim commands for roll and pitch/
+/** Trim commands for roll, pitch and yaw.
+ * These are updated from the trim commands in ap_state via inter_mcu
*/
-#ifndef COMMAND_ROLL_TRIM
-#define COMMAND_ROLL_TRIM 0
-#endif
-
-#ifndef COMMAND_PITCH_TRIM
-#define COMMAND_PITCH_TRIM 0
-#endif
-
pprz_t command_roll_trim;
pprz_t command_pitch_trim;
pprz_t command_yaw_trim;
@@ -141,10 +137,6 @@ void init_fbw( void ) {
fbw_mode = FBW_MODE_FAILSAFE;
- command_roll_trim = COMMAND_ROLL_TRIM;
- command_pitch_trim = COMMAND_PITCH_TRIM;
-
-
/**** start timers for periodic functions *****/
fbw_periodic_tid = sys_time_register_timer((1./60.), NULL);
electrical_tid = sys_time_register_timer(0.1, NULL);
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c
index ba54e696a4..5cc48326e8 100644
--- a/sw/airborne/firmwares/rotorcraft/autopilot.c
+++ b/sw/airborne/firmwares/rotorcraft/autopilot.c
@@ -54,10 +54,25 @@ bool_t kill_throttle;
bool_t autopilot_rc;
bool_t autopilot_power_switch;
-bool_t autopilot_detect_ground;
+bool_t autopilot_ground_detected;
bool_t autopilot_detect_ground_once;
-#define AUTOPILOT_IN_FLIGHT_TIME 40
+#define AUTOPILOT_IN_FLIGHT_TIME 20
+
+/** minimum vertical speed for in_flight condition in m/s */
+#ifndef AUTOPILOT_IN_FLIGHT_MIN_SPEED
+#define AUTOPILOT_IN_FLIGHT_MIN_SPEED 0.2
+#endif
+
+/** minimum vertical acceleration for in_flight condition in m/s^2 */
+#ifndef AUTOPILOT_IN_FLIGHT_MIN_ACCEL
+#define AUTOPILOT_IN_FLIGHT_MIN_ACCEL 2.0
+#endif
+
+/** minimum thrust for in_flight condition in pprz_t units */
+#ifndef AUTOPILOT_IN_FLIGHT_MIN_THRUST
+#define AUTOPILOT_IN_FLIGHT_MIN_THRUST 500
+#endif
#ifndef AUTOPILOT_DISABLE_AHRS_KILL
#include "subsystems/ahrs.h"
@@ -73,10 +88,13 @@ static inline int ahrs_is_aligned(void) {
#if USE_KILL_SWITCH_FOR_MOTOR_ARMING
#include "autopilot_arming_switch.h"
+PRINT_CONFIG_MSG("Using kill switch for motor arming")
#elif USE_THROTTLE_FOR_MOTOR_ARMING
#include "autopilot_arming_throttle.h"
+PRINT_CONFIG_MSG("Using throttle for motor arming")
#else
#include "autopilot_arming_yaw.h"
+PRINT_CONFIG_MSG("Using 2 sec yaw for motor arming")
#endif
#ifndef MODE_STARTUP
@@ -181,7 +199,7 @@ void autopilot_init(void) {
autopilot_in_flight = FALSE;
autopilot_in_flight_counter = 0;
autopilot_mode_auto2 = MODE_AUTO2;
- autopilot_detect_ground = FALSE;
+ autopilot_ground_detected = FALSE;
autopilot_detect_ground_once = FALSE;
autopilot_flight_time = 0;
autopilot_rc = TRUE;
@@ -197,7 +215,7 @@ void autopilot_init(void) {
guidance_v_init();
stabilization_init();
- /* set startup mode, propagats through to guidance h/v */
+ /* set startup mode, propagates through to guidance h/v */
autopilot_set_mode(MODE_STARTUP);
register_periodic_telemetry(DefaultPeriodic, "ALIVE", send_alive);
@@ -216,54 +234,37 @@ void autopilot_init(void) {
}
-static inline void autopilot_check_in_flight_no_rc( bool_t motors_on ) {
- if (autopilot_in_flight) {
- if (autopilot_in_flight_counter > 0) {
- if (stabilization_cmd[COMMAND_THRUST] == 0) {
- autopilot_in_flight_counter--;
- if (autopilot_in_flight_counter == 0) {
- autopilot_in_flight = FALSE;
- }
- }
- else { /* !THROTTLE_STICK_DOWN */
- autopilot_in_flight_counter = AUTOPILOT_IN_FLIGHT_TIME;
- }
- }
- }
- else { /* not in flight */
- if (autopilot_in_flight_counter < AUTOPILOT_IN_FLIGHT_TIME &&
- motors_on) {
- if (stabilization_cmd[COMMAND_THRUST] > 0) {
- autopilot_in_flight_counter++;
- if (autopilot_in_flight_counter == AUTOPILOT_IN_FLIGHT_TIME)
- autopilot_in_flight = TRUE;
- }
- else { /* THROTTLE_STICK_DOWN */
- autopilot_in_flight_counter = 0;
- }
- }
- }
-}
-
-
void autopilot_periodic(void) {
RunOnceEvery(NAV_PRESCALER, nav_periodic_task());
-#if FAILSAFE_GROUND_DETECT
-INFO("Using FAILSAFE_GROUND_DETECT")
- if (autopilot_mode == AP_MODE_FAILSAFE && autopilot_detect_ground) {
- autopilot_set_mode(AP_MODE_KILL);
- autopilot_detect_ground = FALSE;
- }
-#endif
- /* set failsafe commands, if in FAILSAFE or KILL mode */
-#if !FAILSAFE_GROUND_DETECT
- if (autopilot_mode == AP_MODE_KILL ||
- autopilot_mode == AP_MODE_FAILSAFE) {
-#else
- if (autopilot_mode == AP_MODE_KILL) {
+
+ /* If in FAILSAFE mode and either already not in_flight anymore
+ * or just "detected" ground, go to KILL mode.
+ */
+ if (autopilot_mode == AP_MODE_FAILSAFE) {
+ if (!autopilot_in_flight)
+ autopilot_set_mode(AP_MODE_KILL);
+
+#if FAILSAFE_GROUND_DETECT
+INFO("Using FAILSAFE_GROUND_DETECT: KILL")
+ if (autopilot_ground_detected)
+ autopilot_set_mode(AP_MODE_KILL);
#endif
+ }
+
+ /* Reset ground detection _after_ running flight plan
+ */
+ if (!autopilot_in_flight || autopilot_ground_detected) {
+ autopilot_ground_detected = FALSE;
+ autopilot_detect_ground_once = FALSE;
+ }
+
+ /* Set fixed "failsafe" commands from airframe file if in KILL mode.
+ * If in FAILSAFE mode, run normal loops with failsafe attitude and
+ * downwards velocity setpoints.
+ */
+ if (autopilot_mode == AP_MODE_KILL) {
SetCommands(commands_failsafe);
}
else {
@@ -272,10 +273,6 @@ INFO("Using FAILSAFE_GROUND_DETECT")
SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight, autopilot_motors_on);
}
- // when we dont have RC, check in flight by looking at throttle
- if (radio_control.status != RC_OK) {
- autopilot_check_in_flight_no_rc(autopilot_motors_on);
- }
}
@@ -295,7 +292,6 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) {
break;
#endif
case AP_MODE_KILL:
- autopilot_set_motors_on(FALSE);
autopilot_in_flight = FALSE;
autopilot_in_flight_counter = 0;
guidance_h_mode_changed(GUIDANCE_H_MODE_KILL);
@@ -339,6 +335,8 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) {
break;
#endif
case AP_MODE_KILL:
+ autopilot_set_motors_on(FALSE);
+ stabilization_cmd[COMMAND_THRUST] = 0;
guidance_v_mode_changed(GUIDANCE_V_MODE_KILL);
break;
case AP_MODE_RC_DIRECT:
@@ -374,29 +372,37 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) {
}
-static inline void autopilot_check_in_flight( bool_t motors_on ) {
+void autopilot_check_in_flight(bool_t motors_on) {
if (autopilot_in_flight) {
if (autopilot_in_flight_counter > 0) {
- if (THROTTLE_STICK_DOWN()) {
+ /* probably in_flight if thrust, speed and accel above IN_FLIGHT_MIN thresholds */
+ if ((stabilization_cmd[COMMAND_THRUST] <= AUTOPILOT_IN_FLIGHT_MIN_THRUST) &&
+ (abs(stateGetSpeedNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_SPEED) &&
+ (abs(stateGetAccelNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_ACCEL))
+ {
autopilot_in_flight_counter--;
if (autopilot_in_flight_counter == 0) {
autopilot_in_flight = FALSE;
}
}
- else { /* !THROTTLE_STICK_DOWN */
+ else { /* thrust, speed or accel not above min threshold, reset counter */
autopilot_in_flight_counter = AUTOPILOT_IN_FLIGHT_TIME;
}
}
}
- else { /* not in flight */
+ else { /* currently not in flight */
if (autopilot_in_flight_counter < AUTOPILOT_IN_FLIGHT_TIME &&
- motors_on) {
- if (!THROTTLE_STICK_DOWN()) {
+ motors_on)
+ {
+ /* if thrust above min threshold, assume in_flight.
+ * Don't check for velocity and acceleration above threshold here...
+ */
+ if (stabilization_cmd[COMMAND_THRUST] > AUTOPILOT_IN_FLIGHT_MIN_THRUST) {
autopilot_in_flight_counter++;
if (autopilot_in_flight_counter == AUTOPILOT_IN_FLIGHT_TIME)
autopilot_in_flight = TRUE;
}
- else { /* THROTTLE_STICK_DOWN */
+ else { /* currently not in_flight and thrust below threshold, reset counter */
autopilot_in_flight_counter = 0;
}
}
@@ -449,8 +455,6 @@ void autopilot_on_rc_frame(void) {
autopilot_arming_check_motors_on();
kill_throttle = ! autopilot_motors_on;
- autopilot_check_in_flight(autopilot_motors_on);
-
guidance_v_read_rc();
guidance_h_read_rc(autopilot_in_flight);
}
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.h b/sw/airborne/firmwares/rotorcraft/autopilot.h
index 7e874ef0f3..9203c6e43f 100644
--- a/sw/airborne/firmwares/rotorcraft/autopilot.h
+++ b/sw/airborne/firmwares/rotorcraft/autopilot.h
@@ -67,8 +67,9 @@ extern void autopilot_periodic(void);
extern void autopilot_on_rc_frame(void);
extern void autopilot_set_mode(uint8_t new_autopilot_mode);
extern void autopilot_set_motors_on(bool_t motors_on);
+extern void autopilot_check_in_flight(bool_t motors_on);
-extern bool_t autopilot_detect_ground;
+extern bool_t autopilot_ground_detected;
extern bool_t autopilot_detect_ground_once;
extern uint16_t autopilot_flight_time;
@@ -151,7 +152,7 @@ static inline void DetectGroundEvent(void) {
struct NedCoor_f* accel = stateGetAccelNed_f();
if (accel->z < -THRESHOLD_GROUND_DETECT ||
accel->z > THRESHOLD_GROUND_DETECT) {
- autopilot_detect_ground = TRUE;
+ autopilot_ground_detected = TRUE;
autopilot_detect_ground_once = FALSE;
}
}
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
index 1d6d407381..05d1757785 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
@@ -204,25 +204,35 @@ void guidance_h_mode_changed(uint8_t new_mode) {
stabilization_attitude_reset_care_free_heading();
case GUIDANCE_H_MODE_FORWARD:
case GUIDANCE_H_MODE_ATTITUDE:
- stabilization_attitude_enter();
+#if NO_ATTITUDE_RESET_ON_MODE_CHANGE
+ /* reset attitude stabilization if previous mode was not using it */
+ if (guidance_h_mode == GUIDANCE_H_MODE_KILL ||
+ guidance_h_mode == GUIDANCE_H_MODE_RATE ||
+ guidance_h_mode == GUIDANCE_H_MODE_RC_DIRECT)
+#endif
+ stabilization_attitude_enter();
break;
case GUIDANCE_H_MODE_HOVER:
guidance_h_hover_enter();
+#if NO_ATTITUDE_RESET_ON_MODE_CHANGE
/* reset attitude stabilization if previous mode was not using it */
- if (guidance_h_mode == GUIDANCE_H_MODE_RC_DIRECT ||
- guidance_h_mode == GUIDANCE_H_MODE_RATE) {
+ if (guidance_h_mode == GUIDANCE_H_MODE_KILL ||
+ guidance_h_mode == GUIDANCE_H_MODE_RATE ||
+ guidance_h_mode == GUIDANCE_H_MODE_RC_DIRECT)
+#endif
stabilization_attitude_enter();
- }
break;
case GUIDANCE_H_MODE_NAV:
guidance_h_nav_enter();
+#if NO_ATTITUDE_RESET_ON_MODE_CHANGE
/* reset attitude stabilization if previous mode was not using it */
- if (guidance_h_mode == GUIDANCE_H_MODE_RC_DIRECT ||
- guidance_h_mode == GUIDANCE_H_MODE_RATE) {
+ if (guidance_h_mode == GUIDANCE_H_MODE_KILL ||
+ guidance_h_mode == GUIDANCE_H_MODE_RATE ||
+ guidance_h_mode == GUIDANCE_H_MODE_RC_DIRECT)
+#endif
stabilization_attitude_enter();
- }
break;
default:
@@ -314,12 +324,12 @@ void guidance_h_run(bool_t in_flight) {
guidance_h_nav_enter();
if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) {
- struct Int32Eulers sp_euler_i;
- sp_euler_i.phi = nav_roll;
- sp_euler_i.theta = nav_pitch;
+ struct Int32Eulers sp_cmd_i;
+ sp_cmd_i.phi = nav_roll;
+ sp_cmd_i.theta = nav_pitch;
/* FIXME: heading can't be set via attitude block yet, use current heading for now */
- sp_euler_i.psi = stateGetNedToBodyEulers_i()->psi;
- stabilization_attitude_set_from_eulers_i(&sp_euler_i);
+ sp_cmd_i.psi = stateGetNedToBodyEulers_i()->psi;
+ stabilization_attitude_set_cmd_i(&sp_cmd_i);
}
else {
INT32_VECT2_NED_OF_ENU(guidance_h_pos_sp, navigation_carrot);
@@ -328,6 +338,7 @@ void guidance_h_run(bool_t in_flight) {
/* set psi command */
guidance_h_command_body.psi = nav_heading;
+ INT32_ANGLE_NORMALIZE(guidance_h_command_body.psi);
/* compute roll and pitch commands and set final attitude setpoint */
guidance_h_traj_run(in_flight);
}
@@ -423,8 +434,8 @@ static void guidance_h_traj_run(bool_t in_flight) {
guidance_h_command_body.phi += guidance_h_rc_sp.phi;
guidance_h_command_body.theta += guidance_h_rc_sp.theta;
- /* Set attitude setpoint from pseudo-eulers */
- stabilization_attitude_set_from_eulers_i(&guidance_h_command_body);
+ /* Set attitude setpoint from pseudo-euler commands */
+ stabilization_attitude_set_cmd_i(&guidance_h_command_body);
}
static void guidance_h_hover_enter(void) {
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c
index b1db6ebcfa..34b23d212c 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c
@@ -53,7 +53,7 @@ struct Int64Vect2 gh_pos_ref;
#ifndef GUIDANCE_H_REF_MAX_ACCEL
#define GUIDANCE_H_REF_MAX_ACCEL 5.66
#endif
-#define GH_MAX_ACCEL BFP_OF_REAL(GUIDANCE_H_REF_MAX_ACCEL, GH_ACCEL_REF_FRAC)
+static const int32_t gh_max_accel = BFP_OF_REAL(GUIDANCE_H_REF_MAX_ACCEL, GH_ACCEL_REF_FRAC);
/** Speed saturation */
#ifndef GUIDANCE_H_REF_MAX_SPEED
@@ -61,7 +61,7 @@ struct Int64Vect2 gh_pos_ref;
#endif
/** @todo GH_MAX_SPEED must be limited to 2^14 to avoid overflow */
#define GH_MAX_SPEED_REF_FRAC 7
-#define GH_MAX_SPEED BFP_OF_REAL(GUIDANCE_H_REF_MAX_SPEED, GH_MAX_SPEED_REF_FRAC)
+static const int32_t gh_max_speed = BFP_OF_REAL(GUIDANCE_H_REF_MAX_SPEED, GH_MAX_SPEED_REF_FRAC);
/** second order model natural frequency */
#ifndef GUIDANCE_H_REF_OMEGA
@@ -72,14 +72,16 @@ struct Int64Vect2 gh_pos_ref;
#define GUIDANCE_H_REF_ZETA 0.85
#endif
#define GH_ZETA_OMEGA_FRAC 10
-#define GH_ZETA_OMEGA BFP_OF_REAL((GUIDANCE_H_REF_ZETA*GUIDANCE_H_REF_OMEGA), GH_ZETA_OMEGA_FRAC)
#define GH_OMEGA_2_FRAC 7
-#define GH_OMEGA_2 BFP_OF_REAL((GUIDANCE_H_REF_OMEGA*GUIDANCE_H_REF_OMEGA), GH_OMEGA_2_FRAC)
+static const int32_t gh_zeta_omega = BFP_OF_REAL((GUIDANCE_H_REF_ZETA*GUIDANCE_H_REF_OMEGA), GH_ZETA_OMEGA_FRAC);
+static const int32_t gh_omega_2= BFP_OF_REAL((GUIDANCE_H_REF_OMEGA*GUIDANCE_H_REF_OMEGA), GH_OMEGA_2_FRAC);
/** first order time constant */
-#define GH_REF_THAU_F 0.5
-#define GH_REF_INV_THAU_FRAC 16
-#define GH_REF_INV_THAU BFP_OF_REAL((1./GH_REF_THAU_F), GH_REF_INV_THAU_FRAC)
+#ifndef GUIDANCE_H_REF_TAU
+#define GUIDANCE_H_REF_TAU 0.5
+#endif
+#define GH_REF_INV_TAU_FRAC 16
+static const int32_t gh_ref_inv_tau = BFP_OF_REAL((1./GUIDANCE_H_REF_TAU), GH_REF_INV_TAU_FRAC);
static struct Int32Vect2 gh_max_speed_ref;
static struct Int32Vect2 gh_max_accel_ref;
@@ -88,6 +90,13 @@ static int32_t route_ref;
static int32_t s_route_ref;
static int32_t c_route_ref;
+static void gh_compute_route_ref(struct Int32Vect2* ref_vector);
+static void gh_compute_ref_max(struct Int32Vect2* ref_vector);
+static void gh_compute_ref_max_accel(struct Int32Vect2* ref_vector);
+static void gh_compute_ref_max_speed(struct Int32Vect2* ref_vector);
+static void gh_saturate_ref_accel(void);
+static void gh_saturate_ref_speed(void);
+
void gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2 speed, struct Int32Vect2 accel) {
struct Int64Vect2 new_pos;
new_pos.x = ((int64_t)pos.x)<<(GH_POS_REF_FRAC - INT32_POS_FRAC);
@@ -105,7 +114,7 @@ void gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp) {
// compute the "speed part" of accel = -2*zeta*omega*speed -omega^2(pos - pos_sp)
struct Int32Vect2 speed;
INT32_VECT2_RSHIFT(speed, gh_speed_ref, (GH_SPEED_REF_FRAC - GH_ACCEL_REF_FRAC));
- VECT2_SMUL(speed, speed, -2*GH_ZETA_OMEGA);
+ VECT2_SMUL(speed, speed, -2 * gh_zeta_omega);
INT32_VECT2_RSHIFT(speed, speed, GH_ZETA_OMEGA_FRAC);
// compute pos error in pos_sp resolution
struct Int32Vect2 pos_err;
@@ -115,65 +124,19 @@ void gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp) {
INT32_VECT2_RSHIFT(pos_err, pos_err, (INT32_POS_FRAC - GH_ACCEL_REF_FRAC));
// compute the "pos part" of accel
struct Int32Vect2 pos;
- VECT2_SMUL(pos, pos_err, (-GH_OMEGA_2));
+ VECT2_SMUL(pos, pos_err, -gh_omega_2);
INT32_VECT2_RSHIFT(pos, pos, GH_OMEGA_2_FRAC);
// sum accel
VECT2_SUM(gh_accel_ref, speed, pos);
- /* Compute route reference before saturation */
- float f_route_ref = atan2f(-pos_err.y, -pos_err.x);
- route_ref = ANGLE_BFP_OF_REAL(f_route_ref);
- /* Compute North and East route components */
- PPRZ_ITRIG_SIN(s_route_ref, route_ref);
- PPRZ_ITRIG_COS(c_route_ref, route_ref);
- c_route_ref = abs(c_route_ref);
- s_route_ref = abs(s_route_ref);
- /* Compute maximum acceleration*/
- gh_max_accel_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, c_route_ref, INT32_TRIG_FRAC);
- gh_max_accel_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, s_route_ref, INT32_TRIG_FRAC);
- /* Compute maximum speed*/
- gh_max_speed_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, c_route_ref, INT32_TRIG_FRAC);
- gh_max_speed_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, s_route_ref, INT32_TRIG_FRAC);
- /* restore gh_speed_ref range (Q14.17) */
- INT32_VECT2_LSHIFT(gh_max_speed_ref, gh_max_speed_ref, (GH_SPEED_REF_FRAC - GH_MAX_SPEED_REF_FRAC));
+ /* Compute max ref accel/speed along route before saturation */
+ gh_compute_ref_max(&pos_err);
- /* Saturate accelerations */
- if (gh_accel_ref.x <= -gh_max_accel_ref.x) {
- gh_accel_ref.x = -gh_max_accel_ref.x;
- }
- else if (gh_accel_ref.x >= gh_max_accel_ref.x) {
- gh_accel_ref.x = gh_max_accel_ref.x;
- }
- if (gh_accel_ref.y <= -gh_max_accel_ref.y) {
- gh_accel_ref.y = -gh_max_accel_ref.y;
- }
- else if (gh_accel_ref.y >= gh_max_accel_ref.y) {
- gh_accel_ref.y = gh_max_accel_ref.y;
- }
-
- /* Saturate speed and adjust acceleration accordingly */
- if (gh_speed_ref.x <= -gh_max_speed_ref.x) {
- gh_speed_ref.x = -gh_max_speed_ref.x;
- if (gh_accel_ref.x < 0)
- gh_accel_ref.x = 0;
- }
- else if (gh_speed_ref.x >= gh_max_speed_ref.x) {
- gh_speed_ref.x = gh_max_speed_ref.x;
- if (gh_accel_ref.x > 0)
- gh_accel_ref.x = 0;
- }
- if (gh_speed_ref.y <= -gh_max_speed_ref.y) {
- gh_speed_ref.y = -gh_max_speed_ref.y;
- if (gh_accel_ref.y < 0)
- gh_accel_ref.y = 0;
- }
- else if (gh_speed_ref.y >= gh_max_speed_ref.y) {
- gh_speed_ref.y = gh_max_speed_ref.y;
- if (gh_accel_ref.y > 0)
- gh_accel_ref.y = 0;
- }
+ gh_saturate_ref_accel();
+ gh_saturate_ref_speed();
}
+
void gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp) {
/* WARNING: SPEED SATURATION UNTESTED */
VECT2_ADD(gh_pos_ref, gh_speed_ref);
@@ -186,26 +149,80 @@ void gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp) {
// convert to accel resolution
INT32_VECT2_RSHIFT(speed_err, speed_err, (GH_SPEED_REF_FRAC - GH_ACCEL_REF_FRAC));
// compute accel from speed_sp
- VECT2_SMUL(gh_accel_ref, speed_err, -GH_REF_INV_THAU);
- INT32_VECT2_RSHIFT(gh_accel_ref, gh_accel_ref, GH_REF_INV_THAU_FRAC);
+ VECT2_SMUL(gh_accel_ref, speed_err, -gh_ref_inv_tau);
+ INT32_VECT2_RSHIFT(gh_accel_ref, gh_accel_ref, GH_REF_INV_TAU_FRAC);
- /* Compute route reference before saturation */
- float f_route_ref = atan2f(-speed_sp.y, -speed_sp.x);
+ /* Compute max ref accel/speed along route before saturation */
+ gh_compute_ref_max_speed(&speed_sp);
+ gh_compute_ref_max_accel(&speed_err);
+
+ gh_saturate_ref_accel();
+ gh_saturate_ref_speed();
+}
+
+static void gh_compute_route_ref(struct Int32Vect2* ref_vector) {
+ float f_route_ref = atan2f(-ref_vector->y, -ref_vector->x);
route_ref = ANGLE_BFP_OF_REAL(f_route_ref);
/* Compute North and East route components */
PPRZ_ITRIG_SIN(s_route_ref, route_ref);
PPRZ_ITRIG_COS(c_route_ref, route_ref);
c_route_ref = abs(c_route_ref);
s_route_ref = abs(s_route_ref);
- /* Compute maximum acceleration*/
- gh_max_accel_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, c_route_ref, INT32_TRIG_FRAC);
- gh_max_accel_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, s_route_ref, INT32_TRIG_FRAC);
- /* Compute maximum speed*/
- gh_max_speed_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, c_route_ref, INT32_TRIG_FRAC);
- gh_max_speed_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, s_route_ref, INT32_TRIG_FRAC);
- /* restore gh_speed_ref range (Q14.17) */
- INT32_VECT2_LSHIFT(gh_max_speed_ref, gh_max_speed_ref, (GH_SPEED_REF_FRAC - GH_MAX_SPEED_REF_FRAC));
+}
+static void gh_compute_ref_max(struct Int32Vect2* ref_vector) {
+ /* Compute route reference before saturation */
+ if (ref_vector->x == 0 && ref_vector->y == 0) {
+ gh_max_accel_ref.x = 0;
+ gh_max_accel_ref.y = 0;
+ gh_max_speed_ref.x = 0;
+ gh_max_speed_ref.y = 0;
+ }
+ else {
+ gh_compute_route_ref(ref_vector);
+ /* Compute maximum acceleration*/
+ gh_max_accel_ref.x = INT_MULT_RSHIFT(gh_max_accel, c_route_ref, INT32_TRIG_FRAC);
+ gh_max_accel_ref.y = INT_MULT_RSHIFT(gh_max_accel, s_route_ref, INT32_TRIG_FRAC);
+ /* Compute maximum speed*/
+ gh_max_speed_ref.x = INT_MULT_RSHIFT(gh_max_speed, c_route_ref, INT32_TRIG_FRAC);
+ gh_max_speed_ref.y = INT_MULT_RSHIFT(gh_max_speed, s_route_ref, INT32_TRIG_FRAC);
+ /* restore gh_speed_ref range (Q14.17) */
+ INT32_VECT2_LSHIFT(gh_max_speed_ref, gh_max_speed_ref, (GH_SPEED_REF_FRAC - GH_MAX_SPEED_REF_FRAC));
+ }
+}
+
+static void gh_compute_ref_max_accel(struct Int32Vect2* ref_vector) {
+ /* Compute route reference before saturation */
+ if (ref_vector->x == 0 && ref_vector->y == 0) {
+ gh_max_accel_ref.x = 0;
+ gh_max_accel_ref.y = 0;
+ }
+ else {
+ gh_compute_route_ref(ref_vector);
+ /* Compute maximum acceleration*/
+ gh_max_accel_ref.x = INT_MULT_RSHIFT(gh_max_accel, c_route_ref, INT32_TRIG_FRAC);
+ gh_max_accel_ref.y = INT_MULT_RSHIFT(gh_max_accel, s_route_ref, INT32_TRIG_FRAC);
+ }
+}
+
+static void gh_compute_ref_max_speed(struct Int32Vect2* ref_vector) {
+ /* Compute route reference before saturation */
+ if (ref_vector->x == 0 && ref_vector->y == 0) {
+ gh_max_speed_ref.x = 0;
+ gh_max_speed_ref.y = 0;
+ }
+ else {
+ gh_compute_route_ref(ref_vector);
+ /* Compute maximum speed*/
+ gh_max_speed_ref.x = INT_MULT_RSHIFT(gh_max_speed, c_route_ref, INT32_TRIG_FRAC);
+ gh_max_speed_ref.y = INT_MULT_RSHIFT(gh_max_speed, s_route_ref, INT32_TRIG_FRAC);
+ /* restore gh_speed_ref range (Q14.17) */
+ INT32_VECT2_LSHIFT(gh_max_speed_ref, gh_max_speed_ref, (GH_SPEED_REF_FRAC - GH_MAX_SPEED_REF_FRAC));
+ }
+}
+
+/** saturate reference accelerations */
+static void gh_saturate_ref_accel(void) {
/* Saturate accelerations */
if (gh_accel_ref.x <= -gh_max_accel_ref.x) {
gh_accel_ref.x = -gh_max_accel_ref.x;
@@ -219,8 +236,10 @@ void gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp) {
else if (gh_accel_ref.y >= gh_max_accel_ref.y) {
gh_accel_ref.y = gh_max_accel_ref.y;
}
+}
- /* Saturate speed and adjust acceleration accordingly */
+/** Saturate ref speed and adjust acceleration accordingly */
+static void gh_saturate_ref_speed(void) {
if (gh_speed_ref.x <= -gh_max_speed_ref.x) {
gh_speed_ref.x = -gh_max_speed_ref.x;
if (gh_accel_ref.x < 0)
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
index 10f23cdddc..82e990fe8a 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
@@ -24,32 +24,43 @@
*
*/
-#define GUIDANCE_V_C
+#include "generated/airframe.h"
#include "firmwares/rotorcraft/guidance/guidance_v.h"
-
#include "subsystems/radio_control.h"
#include "firmwares/rotorcraft/stabilization.h"
-// #include "booz_fms.h" FIXME
#include "firmwares/rotorcraft/navigation.h"
#include "state.h"
#include "math/pprz_algebra_int.h"
-#include "generated/airframe.h"
-
-/* warn if some gains are still negative */
+/* error if some gains are negative */
#if (GUIDANCE_V_HOVER_KP < 0) || \
(GUIDANCE_V_HOVER_KD < 0) || \
(GUIDANCE_V_HOVER_KI < 0)
-#warning "ALL control gains are now positive!!!"
+#error "ALL control gains must be positive!!!"
#endif
-#if defined GUIDANCE_V_INV_M
-#warning "GUIDANCE_V_INV_M has been removed. If you don't want to use adaptive hover, please define GUIDANCE_V_NOMINAL_HOVER_THROTTLE"
+
+/* If only GUIDANCE_V_NOMINAL_HOVER_THROTTLE is defined,
+ * disable the adaptive throttle estimation by default.
+ * Otherwise enable adaptive estimation by default.
+ */
+#ifdef GUIDANCE_V_NOMINAL_HOVER_THROTTLE
+# ifndef GUIDANCE_V_ADAPT_THROTTLE_ENABLED
+# define GUIDANCE_V_ADAPT_THROTTLE_ENABLED FALSE
+# endif
+#else
+# define GUIDANCE_V_NOMINAL_HOVER_THROTTLE 0.4
+# ifndef GUIDANCE_V_ADAPT_THROTTLE_ENABLED
+# define GUIDANCE_V_ADAPT_THROTTLE_ENABLED TRUE
+# endif
#endif
+PRINT_CONFIG_VAR(GUIDANCE_V_NOMINAL_HOVER_THROTTLE)
+PRINT_CONFIG_VAR(GUIDANCE_V_ADAPT_THROTTLE_ENABLED)
+
uint8_t guidance_v_mode;
int32_t guidance_v_ff_cmd;
@@ -57,6 +68,7 @@ int32_t guidance_v_fb_cmd;
int32_t guidance_v_delta_t;
float guidance_v_nominal_throttle;
+bool_t guidance_v_adapt_throttle_enabled;
/** Direct throttle from radio control.
@@ -82,6 +94,8 @@ int32_t guidance_v_ki;
int32_t guidance_v_z_sum_err;
+static int32_t guidance_v_thrust_coeff;
+
#define GuidanceVSetRef(_pos, _speed, _accel) { \
gv_set_ref(_pos, _speed, _accel); \
@@ -90,8 +104,8 @@ int32_t guidance_v_z_sum_err;
guidance_v_zdd_ref = _accel; \
}
-
-__attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flight);
+static int32_t get_vertical_thrust_coeff(void);
+static void run_hover_loop(bool_t in_flight);
#if DOWNLINK
#include "subsystems/datalink/telemetry.h"
@@ -112,6 +126,14 @@ static void send_vert_loop(void) {
&guidance_v_fb_cmd,
&guidance_v_delta_t);
}
+
+static void send_tune_vert(void) {
+ DOWNLINK_SEND_TUNE_VERT(DefaultChannel, DefaultDevice,
+ &guidance_v_z_sp,
+ &(stateGetPositionNed_i()->z),
+ &guidance_v_z_ref,
+ &guidance_v_zd_ref);
+}
#endif
void guidance_v_init(void) {
@@ -124,14 +146,14 @@ void guidance_v_init(void) {
guidance_v_z_sum_err = 0;
-#ifdef GUIDANCE_V_NOMINAL_HOVER_THROTTLE
guidance_v_nominal_throttle = GUIDANCE_V_NOMINAL_HOVER_THROTTLE;
-#endif
+ guidance_v_adapt_throttle_enabled = GUIDANCE_V_ADAPT_THROTTLE_ENABLED;
gv_adapt_init();
#if DOWNLINK
register_periodic_telemetry(DefaultPeriodic, "VERT_LOOP", send_vert_loop);
+ register_periodic_telemetry(DefaultPeriodic, "TUNE_VERT", send_vert_loop);
#endif
}
@@ -187,8 +209,14 @@ void guidance_v_run(bool_t in_flight) {
// FIXME... SATURATIONS NOT TAKEN INTO ACCOUNT
// AKA SUPERVISION and co
+ guidance_v_thrust_coeff = get_vertical_thrust_coeff();
if (in_flight) {
- gv_adapt_run(stateGetAccelNed_i()->z, stabilization_cmd[COMMAND_THRUST], guidance_v_zd_ref);
+ int32_t vertical_thrust = (stabilization_cmd[COMMAND_THRUST] * guidance_v_thrust_coeff) >> INT32_TRIG_FRAC;
+ gv_adapt_run(stateGetAccelNed_i()->z, vertical_thrust, guidance_v_zd_ref);
+ }
+ else {
+ /* reset estimate while not in_flight */
+ gv_adapt_init();
}
switch (guidance_v_mode) {
@@ -226,6 +254,7 @@ void guidance_v_run(bool_t in_flight) {
if (fms.enabled && fms.input.v_mode == GUIDANCE_V_MODE_HOVER)
guidance_v_z_sp = fms.input.v_sp.height;
#endif
+ guidance_v_zd_sp = 0;
gv_update_ref_from_z_sp(guidance_v_z_sp);
run_hover_loop(in_flight);
#if NO_RC_THRUST_LIMIT
@@ -240,17 +269,21 @@ void guidance_v_run(bool_t in_flight) {
{
if (vertical_mode == VERTICAL_MODE_ALT) {
guidance_v_z_sp = -nav_flight_altitude;
+ guidance_v_zd_sp = 0;
gv_update_ref_from_z_sp(guidance_v_z_sp);
run_hover_loop(in_flight);
}
else if (vertical_mode == VERTICAL_MODE_CLIMB) {
+ guidance_v_z_sp = stateGetPositionNed_i()->z;
guidance_v_zd_sp = -nav_climb;
gv_update_ref_from_zd_sp(guidance_v_zd_sp);
- nav_flight_altitude = -guidance_v_z_sp;
run_hover_loop(in_flight);
}
else if (vertical_mode == VERTICAL_MODE_MANUAL) {
- guidance_v_z_sp = -nav_flight_altitude; // For display only
+ guidance_v_z_sp = stateGetPositionNed_i()->z;
+ guidance_v_zd_sp = stateGetSpeedNed_i()->z;
+ GuidanceVSetRef(guidance_v_z_sp, guidance_v_zd_sp, 0);
+ guidance_v_z_sum_err = 0;
guidance_v_delta_t = nav_throttle;
}
#if NO_RC_THRUST_LIMIT
@@ -269,12 +302,35 @@ void guidance_v_run(bool_t in_flight) {
}
}
+/// get the cosine of the angle between thrust vector and gravity vector
+static int32_t get_vertical_thrust_coeff(void) {
+ static const int32_t max_bank_coef = BFP_OF_REAL(RadOfDeg(30.), INT32_TRIG_FRAC);
+
+ struct Int32RMat* att = stateGetNedToBodyRMat_i();
+ /* thrust vector:
+ * INT32_RMAT_VMULT(thrust_vect, att, zaxis)
+ * same as last colum of rmat with INT32_TRIG_FRAC
+ * struct Int32Vect thrust_vect = {att.m[2], att.m[5], att.m[8]};
+ *
+ * Angle between two vectors v1 and v2:
+ * angle = acos(dot(v1, v2) / (norm(v1) * norm(v2)))
+ * since here both are already of unit length:
+ * angle = acos(dot(v1, v2))
+ * since we we want the cosine of the angle we simply need
+ * thrust_coeff = dot(v1, v2)
+ * also can be simplified considering: v1 is zaxis with (0,0,1)
+ * dot(v1, v2) = v1.z * v2.z = v2.z
+ */
+ int32_t coef = att->m[8];
+ if (coef < max_bank_coef)
+ coef = max_bank_coef;
+ return coef;
+}
+
#define FF_CMD_FRAC 18
-#define MAX_BANK_COEF (BFP_OF_REAL(RadOfDeg(30.),INT32_TRIG_FRAC))
-
-__attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flight) {
+static void run_hover_loop(bool_t in_flight) {
/* convert our reference to generic representation */
int64_t tmp = gv_z_ref>>(GV_Z_REF_FRAC - INT32_POS_FRAC);
@@ -295,23 +351,21 @@ __attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flig
guidance_v_z_sum_err = 0;
/* our nominal command : (g + zdd)*m */
-#ifdef GUIDANCE_V_NOMINAL_HOVER_THROTTLE
- const int32_t inv_m = BFP_OF_REAL(9.81/(guidance_v_nominal_throttle*MAX_PPRZ), FF_CMD_FRAC);
-#else
- const int32_t inv_m = gv_adapt_X>>(GV_ADAPT_X_FRAC - FF_CMD_FRAC);
-#endif
+ int32_t inv_m;
+ if (guidance_v_adapt_throttle_enabled) {
+ inv_m = gv_adapt_X >> (GV_ADAPT_X_FRAC - FF_CMD_FRAC);
+ }
+ else {
+ /* use the fixed nominal throttle */
+ inv_m = BFP_OF_REAL(9.81 / (guidance_v_nominal_throttle * MAX_PPRZ), FF_CMD_FRAC);
+ }
+
const int32_t g_m_zdd = (int32_t)BFP_OF_REAL(9.81, FF_CMD_FRAC) -
- (guidance_v_zdd_ref<<(FF_CMD_FRAC - INT32_ACCEL_FRAC));
+ (guidance_v_zdd_ref << (FF_CMD_FRAC - INT32_ACCEL_FRAC));
guidance_v_ff_cmd = g_m_zdd / inv_m;
- int32_t cphi,ctheta,cphitheta;
- struct Int32Eulers* att_euler = stateGetNedToBodyEulers_i();
- PPRZ_ITRIG_COS(cphi, att_euler->phi);
- PPRZ_ITRIG_COS(ctheta, att_euler->theta);
- cphitheta = (cphi * ctheta) >> INT32_TRIG_FRAC;
- if (cphitheta < MAX_BANK_COEF) cphitheta = MAX_BANK_COEF;
/* feed forward command */
- guidance_v_ff_cmd = (guidance_v_ff_cmd << INT32_TRIG_FRAC) / cphitheta;
+ guidance_v_ff_cmd = (guidance_v_ff_cmd << INT32_TRIG_FRAC) / guidance_v_thrust_coeff;
/* bound the nominal command to 0.9*MAX_PPRZ */
Bound(guidance_v_ff_cmd, 0, 8640);
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h
index 56c2437215..4a94b4cfa8 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h
@@ -24,15 +24,13 @@
*
*/
-#ifndef GUIDANCE_V
-#define GUIDANCE_V
+#ifndef GUIDANCE_V_H
+#define GUIDANCE_V_H
#include "std.h"
-#include "generated/airframe.h"
#include "firmwares/rotorcraft/guidance/guidance_v_ref.h"
-
-#include "firmwares/rotorcraft/guidance/guidance_v_adpt.h"
+#include "firmwares/rotorcraft/guidance/guidance_v_adapt.h"
#define GUIDANCE_V_MODE_KILL 0
#define GUIDANCE_V_MODE_RC_DIRECT 1
@@ -84,11 +82,15 @@ extern int32_t guidance_v_fb_cmd; ///< feed-back command
extern int32_t guidance_v_delta_t;
/** nominal throttle for hover.
- * This is only used if #"GUIDANCE_V_NOMINAL_HOVER_THROTTLE is defined!
+ * This is only used if #GUIDANCE_V_NOMINAL_HOVER_THROTTLE is defined!
* Unit: factor of #MAX_PPRZ with range 0.1 : 0.9
*/
extern float guidance_v_nominal_throttle;
+/** Use adaptive throttle command estimation.
+ */
+extern bool_t guidance_v_adapt_throttle_enabled;
+
extern int32_t guidance_v_kp; ///< vertical control P-gain
extern int32_t guidance_v_kd; ///< vertical control D-gain
extern int32_t guidance_v_ki; ///< vertical control I-gain
@@ -104,4 +106,4 @@ extern void guidance_v_run(bool_t in_flight);
guidance_v_z_sum_err = 0; \
}
-#endif /* GUIDANCE_V */
+#endif /* GUIDANCE_V_H */
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adapt.c
similarity index 57%
rename from sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h
rename to sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adapt.c
index b22a8c188b..e1cd5e4979 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adapt.c
@@ -1,5 +1,5 @@
/*
- * Copyright (C) 2009-2010 The Paparazzi Team
+ * Copyright (C) 2009-2013 The Paparazzi Team
*
* This file is part of paparazzi.
*
@@ -19,36 +19,58 @@
* Boston, MA 02111-1307, USA.
*/
-/** @file firmwares/rotorcraft/guidance/guidance_v_adpt.h
- * Adaptation bloc of the vertical guidance.
+/** @file firmwares/rotorcraft/guidance/guidance_v_adapt.c
+ * Adaptation block of the vertical guidance.
*
* This is a dimension one kalman filter estimating
- * the ratio of vertical acceleration over thrust command ( ~ invert of the mass )
- * needed by the invert dynamic model to produce a nominal command
+ * the ratio of vertical acceleration over thrust command ( ~ inverse of the mass )
+ * needed by the invert dynamic model to produce a nominal command.
*/
-#ifndef GUIDANCE_V_ADPT
-#define GUIDANCE_V_ADPT
-
+#include "firmwares/rotorcraft/guidance/guidance_v_adapt.h"
#include "paparazzi.h"
+#include "math/pprz_algebra_int.h"
+#include "generated/airframe.h"
+
+
+/** Initial hover throttle as factor of MAX_PPRZ.
+ * Should be a value between #GUIDANCE_V_ADAPT_MIN_HOVER_THROTTLE and
+ * #GUIDANCE_V_ADAPT_MAX_HOVER_THROTTLE.
+ * It is better to start with low thrust and let it rise as the adaptive filter
+ * finds the vehicle needs more thrust.
+ */
+#ifndef GUIDANCE_V_ADAPT_INITIAL_HOVER_THROTTLE
+#define GUIDANCE_V_ADAPT_INITIAL_HOVER_THROTTLE 0.3
+#endif
+PRINT_CONFIG_VAR(GUIDANCE_V_ADAPT_INITIAL_HOVER_THROTTLE)
+
+/** Minimum hover throttle as factor of MAX_PPRZ.
+ * With the default of 0.2 the nominal hover throttle will
+ * never go lower than 20%.
+ */
+#ifndef GUIDANCE_V_ADAPT_MIN_HOVER_THROTTLE
+#define GUIDANCE_V_ADAPT_MIN_HOVER_THROTTLE 0.2
+#endif
+PRINT_CONFIG_VAR(GUIDANCE_V_ADAPT_MIN_HOVER_THROTTLE)
+
+/** Maximum hover throttle as factor of MAX_PPRZ.
+ * With the default of 0.75 the nominal hover throttle will
+ * never go over 75% of max throttle.
+ */
+#ifndef GUIDANCE_V_ADAPT_MAX_HOVER_THROTTLE
+#define GUIDANCE_V_ADAPT_MAX_HOVER_THROTTLE 0.75
+#endif
+PRINT_CONFIG_VAR(GUIDANCE_V_ADAPT_MAX_HOVER_THROTTLE)
/** Adapt noise factor.
- * Smaller values will make the filter to adapter faster.
- * Bigger values (slower adaptation) make the filter more robust to external perturbations.
+ * Smaller values will make the filter to adapt faster.
+ * Bigger values (slower adaptation) make the filter more robust to external pertubations.
* Factor should always be >0
*/
#ifndef GUIDANCE_V_ADAPT_NOISE_FACTOR
#define GUIDANCE_V_ADAPT_NOISE_FACTOR 1.0
#endif
-/** Initial estimation.
- * The initial value can be adapted for faster converging time.
- * It is usually recommended to start with a low value (overestimation of the mass),
- * as it is helping for a smooth takeoff.
- */
-#ifndef GUIDANCE_V_ADAPT_X0
-#define GUIDANCE_V_ADAPT_X0 0.003
-#endif
/** Filter is not fed if accel values are more than +/- MAX_ACCEL.
* MAX_ACCEL is a positive value in m/s^2
@@ -67,37 +89,12 @@
#define GUIDANCE_V_ADAPT_MIN_CMD 0.1
#endif
-/** State of the estimator.
- * fixed point representation with #GV_ADAPT_X_FRAC
- * Q13.18
- */
-extern int32_t gv_adapt_X;
-#define GV_ADAPT_X_FRAC 24
-/** Covariance.
- * fixed point representation with #GV_ADAPT_P_FRAC
- * Q13.18
- */
-extern int32_t gv_adapt_P;
-#define GV_ADAPT_P_FRAC 18
-
-/** Measurement */
-extern int32_t gv_adapt_Xmeas;
-
-
-#ifdef GUIDANCE_V_C
int32_t gv_adapt_X;
int32_t gv_adapt_P;
int32_t gv_adapt_Xmeas;
-
-/* Initial State and Covariance */
-#define GV_ADAPT_X0_F GUIDANCE_V_ADAPT_X0
-#define GV_ADAPT_X0 BFP_OF_REAL(GV_ADAPT_X0_F, GV_ADAPT_X_FRAC)
-#define GV_ADAPT_P0_F 0.1
-#define GV_ADAPT_P0 BFP_OF_REAL(GV_ADAPT_P0_F, GV_ADAPT_P_FRAC)
-
/* System noises */
#ifndef GV_ADAPT_SYS_NOISE_F
#define GV_ADAPT_SYS_NOISE_F 0.00005
@@ -109,31 +106,15 @@ int32_t gv_adapt_Xmeas;
#define GV_ADAPT_MEAS_NOISE_HOVER BFP_OF_REAL(GV_ADAPT_MEAS_NOISE_HOVER_F, GV_ADAPT_P_FRAC)
#define GV_ADAPT_MEAS_NOISE_OF_ZD (100.0*GUIDANCE_V_ADAPT_NOISE_FACTOR)
-/* Max accel */
-#define GV_ADAPT_MAX_ACCEL ACCEL_BFP_OF_REAL(GUIDANCE_V_ADAPT_MAX_ACCEL)
+/* Initial Covariance */
+#define GV_ADAPT_P0_F 0.1
+static const int32_t gv_adapt_P0 = BFP_OF_REAL(GV_ADAPT_P0_F, GV_ADAPT_P_FRAC);
+static const int32_t gv_adapt_X0 = BFP_OF_REAL(9.81, GV_ADAPT_X_FRAC) /
+ (GUIDANCE_V_ADAPT_INITIAL_HOVER_THROTTLE * MAX_PPRZ);
-/* Command bounds */
-#define GV_ADAPT_MAX_CMD ((int32_t)(GUIDANCE_V_ADAPT_MAX_CMD*MAX_PPRZ))
-#define GV_ADAPT_MIN_CMD ((int32_t)(GUIDANCE_V_ADAPT_MIN_CMD*MAX_PPRZ))
-
-/* Output bounds.
- * Don't let it climb over a value that would
- * give less than zero throttle and don't let it down to zero.
- * Worst cases:
- * MIN_ACCEL / MAX_THROTTLE
- * MAX_ACCEL / MIN_THROTTLE
- * ex:
- * 9.81*2^18/255 = 10084
- * 9.81*2^18/1 = 2571632
- */
-// TODO Check this properly
-#define GV_ADAPT_MAX_OUT (BFP_OF_REAL(9.81, GV_ADAPT_X_FRAC))
-#define GV_ADAPT_MIN_OUT (BFP_OF_REAL(9.81, GV_ADAPT_X_FRAC) / MAX_PPRZ)
-
-
-static inline void gv_adapt_init(void) {
- gv_adapt_X = GV_ADAPT_X0;
- gv_adapt_P = GV_ADAPT_P0;
+void gv_adapt_init(void) {
+ gv_adapt_X = gv_adapt_X0;
+ gv_adapt_P = gv_adapt_P0;
}
#define K_FRAC 12
@@ -143,12 +124,16 @@ static inline void gv_adapt_init(void) {
* @param thrust_applied controller input [0 : MAX_PPRZ]
* @param zd_ref vertical speed reference in m/s with #INT32_SPEED_FRAC
*/
-static inline void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_t zd_ref) {
+void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_t zd_ref) {
+
+ static const int32_t gv_adapt_min_cmd = GUIDANCE_V_ADAPT_MIN_CMD * MAX_PPRZ;
+ static const int32_t gv_adapt_max_cmd = GUIDANCE_V_ADAPT_MAX_CMD * MAX_PPRZ;
+ static const int32_t gv_adapt_max_accel = ACCEL_BFP_OF_REAL(GUIDANCE_V_ADAPT_MAX_ACCEL);
/* Update only if accel and commands are in a valid range */
/* This also ensures we don't divide by zero */
- if (thrust_applied < GV_ADAPT_MIN_CMD || thrust_applied > GV_ADAPT_MAX_CMD
- || zdd_meas < -GV_ADAPT_MAX_ACCEL || zdd_meas > GV_ADAPT_MAX_ACCEL) {
+ if (thrust_applied < gv_adapt_min_cmd || thrust_applied > gv_adapt_max_cmd
+ || zdd_meas < -gv_adapt_max_accel || zdd_meas > gv_adapt_max_accel) {
return;
}
@@ -178,18 +163,21 @@ static inline void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_
/* Update Covariance Pnew = P - K * P */
gv_adapt_P = gv_adapt_P - ((K * gv_adapt_P)>>K_FRAC);
/* Don't let covariance climb over initial value */
- if (gv_adapt_P > GV_ADAPT_P0) gv_adapt_P = GV_ADAPT_P0;
+ if (gv_adapt_P > gv_adapt_P0) {
+ gv_adapt_P = gv_adapt_P0;
+ }
/* Update State */
gv_adapt_X = gv_adapt_X + (((int64_t)(K * residual))>>K_FRAC);
- /* Again don't let it climb over a value that would
- * give less than zero throttle and don't let it down to zero.
+ /* Output bounds.
+ * Don't let it climb over a value that would
+ * give less than #GUIDANCE_V_ADAPT_MIN_HOVER_THROTTLE % throttle
+ * or more than #GUIDANCE_V_ADAPT_MAX_HOVER_THROTTLE % throttle.
*/
- Bound(gv_adapt_X, GV_ADAPT_MIN_OUT, GV_ADAPT_MAX_OUT);
+ static const int32_t max_out = BFP_OF_REAL(9.81, GV_ADAPT_X_FRAC) /
+ (GUIDANCE_V_ADAPT_MIN_HOVER_THROTTLE * MAX_PPRZ);
+ static const int32_t min_out = BFP_OF_REAL(9.81, GV_ADAPT_X_FRAC) /
+ (GUIDANCE_V_ADAPT_MAX_HOVER_THROTTLE * MAX_PPRZ);
+ Bound(gv_adapt_X, min_out, max_out);
}
-
-
-#endif /* GUIDANCE_V_C */
-
-#endif /* GUIDANCE_V_ADPT */
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adapt.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adapt.h
new file mode 100644
index 0000000000..8be2e35a22
--- /dev/null
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adapt.h
@@ -0,0 +1,57 @@
+/*
+ * Copyright (C) 2009-2013 The Paparazzi Team
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/** @file firmwares/rotorcraft/guidance/guidance_v_adapt.h
+ * Adaptation block of the vertical guidance.
+ *
+ * This is a dimension one kalman filter estimating
+ * the ratio of vertical acceleration over thrust command ( ~ inverse of the mass )
+ * needed by the invert dynamic model to produce a nominal command.
+ */
+
+#ifndef GUIDANCE_V_ADAPT_H
+#define GUIDANCE_V_ADAPT_H
+
+#include "std.h"
+
+/** State of the estimator.
+ * fixed point representation with #GV_ADAPT_X_FRAC
+ * Q13.18
+ */
+extern int32_t gv_adapt_X;
+#define GV_ADAPT_X_FRAC 24
+
+/** Covariance.
+ * fixed point representation with #GV_ADAPT_P_FRAC
+ * Q13.18
+ */
+extern int32_t gv_adapt_P;
+#define GV_ADAPT_P_FRAC 18
+
+/** Measurement */
+extern int32_t gv_adapt_Xmeas;
+
+
+extern void gv_adapt_init(void);
+extern void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_t zd_ref);
+
+
+#endif /* GUIDANCE_V_ADAPT_H */
diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c
index a1ed2a3c64..b1d531ab8a 100644
--- a/sw/airborne/firmwares/rotorcraft/main.c
+++ b/sw/airborne/firmwares/rotorcraft/main.c
@@ -79,14 +79,14 @@
/* if PRINT_CONFIG is defined, print some config options */
PRINT_CONFIG_VAR(PERIODIC_FREQUENCY)
-#ifndef TELEMETRY_FREQUENCY
-#define TELEMETRY_FREQUENCY 60
-#endif
+/* TELEMETRY_FREQUENCY is defined in generated/periodic_telemetry.h
+ * defaults to 60Hz or set by TELEMETRY_FREQUENCY configure option in airframe file
+ */
PRINT_CONFIG_VAR(TELEMETRY_FREQUENCY)
-#ifndef MODULES_FREQUENCY
-#define MODULES_FREQUENCY 512
-#endif
+/* MODULES_FREQUENCY is defined in generated/modules.h
+ * according to main_freq parameter set for modules in airframe file
+ */
PRINT_CONFIG_VAR(MODULES_FREQUENCY)
#ifndef BARO_PERIODIC_FREQUENCY
@@ -140,7 +140,9 @@ STATIC_INLINE void main_init( void ) {
baro_init();
imu_init();
-
+#if USE_IMU_FLOAT
+ imu_float_init();
+#endif
ahrs_aligner_init();
ahrs_init();
@@ -222,6 +224,14 @@ STATIC_INLINE void failsafe_check( void ) {
autopilot_set_mode(AP_MODE_FAILSAFE);
}
+#if FAILSAFE_ON_BAT_CRITICAL
+ if (autopilot_mode != AP_MODE_KILL &&
+ electrical.bat_critical)
+ {
+ autopilot_set_mode(AP_MODE_FAILSAFE);
+ }
+#endif
+
#if USE_GPS
if (autopilot_mode == AP_MODE_NAV &&
autopilot_motors_on &&
@@ -233,6 +243,8 @@ STATIC_INLINE void failsafe_check( void ) {
autopilot_set_mode(AP_MODE_FAILSAFE);
}
#endif
+
+ autopilot_check_in_flight(autopilot_motors_on);
}
STATIC_INLINE void main_event( void ) {
@@ -284,6 +296,9 @@ static inline void on_gyro_event( void ) {
if (nps_bypass_ahrs) sim_overwrite_ahrs();
#endif
ins_propagate();
+#ifdef SITL
+ if (nps_bypass_ins) sim_overwrite_ins();
+#endif
}
#ifdef USE_VEHICLE_INTERFACE
vi_notify_imu_available();
@@ -302,6 +317,7 @@ static inline void on_baro_dif_event( void ) {
}
static inline void on_gps_event(void) {
+ ahrs_update_gps();
ins_update_gps();
#ifdef USE_VEHICLE_INTERFACE
if (gps.fix == GPS_FIX_3D)
diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c
index de2b555c4c..30bcb9fd6b 100644
--- a/sw/airborne/firmwares/rotorcraft/navigation.c
+++ b/sw/airborne/firmwares/rotorcraft/navigation.c
@@ -374,9 +374,13 @@ void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int
}
bool_t nav_detect_ground(void) {
- if (!autopilot_detect_ground) return FALSE;
- autopilot_detect_ground = FALSE;
+ if (!autopilot_ground_detected) return FALSE;
+ autopilot_ground_detected = FALSE;
return TRUE;
}
+bool_t nav_is_in_flight(void) {
+ return autopilot_in_flight;
+}
+
void nav_home(void) {}
diff --git a/sw/airborne/firmwares/rotorcraft/navigation.h b/sw/airborne/firmwares/rotorcraft/navigation.h
index e992f9f311..b8df4ee553 100644
--- a/sw/airborne/firmwares/rotorcraft/navigation.h
+++ b/sw/airborne/firmwares/rotorcraft/navigation.h
@@ -81,6 +81,7 @@ unit_t nav_reset_alt( void ) __attribute__ ((unused));
void nav_periodic_task(void);
void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i * new_pos);
bool_t nav_detect_ground(void);
+bool_t nav_is_in_flight(void);
void nav_home(void);
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h
index 7a0572f69e..94b47466e4 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h
@@ -34,7 +34,7 @@ extern void stabilization_attitude_init(void);
extern void stabilization_attitude_read_rc(bool_t in_flight);
extern void stabilization_attitude_enter(void);
extern void stabilization_attitude_set_failsafe_setpoint(void);
-extern void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler);
+extern void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd);
extern void stabilization_attitude_run(bool_t in_flight);
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c
index b71c846bf5..f70a684093 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c
@@ -41,7 +41,7 @@ float stabilization_att_ff_cmd[COMMANDS_NB];
static void send_att(void) {
struct FloatRates* body_rate = stateGetBodyRates_i();
struct FloatEulers* att = stateGetNedToBodyEulers_i();
- float foo;
+ float foo = 0.0;
DOWNLINK_SEND_STAB_ATTITUDE_FLOAT(DefaultChannel, DefaultDevice,
&(body_rate->p), &(body_rate->q), &(body_rate->r),
&(att->phi), &(att->theta), &(att->psi),
@@ -134,8 +134,8 @@ void stabilization_attitude_set_failsafe_setpoint(void) {
stab_att_sp_euler.psi = stateGetNedToBodyEulers_f()->psi;
}
-void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler) {
- EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_euler);
+void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) {
+ EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_cmd);
}
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
index e3deb37a7a..47260a4928 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
@@ -149,8 +149,8 @@ void stabilization_attitude_set_failsafe_setpoint(void) {
stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi;
}
-void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler) {
- memcpy(&stab_att_sp_euler, sp_euler, sizeof(struct Int32Eulers));
+void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) {
+ memcpy(&stab_att_sp_euler, sp_cmd, sizeof(struct Int32Eulers));
}
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c
index 69f7004aca..5ddcb0e7c3 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c
@@ -78,7 +78,7 @@ void stabilization_attitude_set_failsafe_setpoint(void) {
stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi;
}
-void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler) {
- memcpy(&stab_att_sp_euler, sp_euler, sizeof(struct Int32Eulers));
+void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) {
+ memcpy(&stab_att_sp_euler, sp_cmd, sizeof(struct Int32Eulers));
}
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 e546692a87..a5764fa859 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
@@ -142,9 +142,74 @@ void stabilization_attitude_set_failsafe_setpoint(void) {
stab_att_sp_quat.qz = sinf(heading2);
}
-void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler) {
- EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_euler);
- FLOAT_QUAT_OF_EULERS(stab_att_sp_quat, stab_att_sp_euler);
+void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) {
+ EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_cmd);
+
+ /* orientation vector describing simultaneous rotation of roll/pitch */
+ struct FloatVect3 ov;
+ ov.x = stab_att_sp_euler.phi;
+ ov.y = stab_att_sp_euler.theta;
+ ov.z = 0.0;
+ /* quaternion from that orientation vector */
+ struct FloatQuat q_rp;
+ FLOAT_QUAT_OF_ORIENTATION_VECT(q_rp, ov);
+
+ /// @todo optimize yaw angle calculation
+
+ /*
+ * Instead of using the psi setpoint angle to rotate around the body z-axis,
+ * calculate the real angle needed to align the projection of the body x-axis
+ * onto the horizontal plane with the psi setpoint.
+ *
+ * angle between two vectors a and b:
+ * angle = atan2(norm(cross(a,b)), dot(a,b)) * sign(dot(cross(a,b), n))
+ * where n is the thrust vector (i.e. both a and b lie in that plane)
+ */
+ const struct FloatVect3 xaxis = {1.0, 0.0, 0.0};
+ const struct FloatVect3 zaxis = {0.0, 0.0, 1.0};
+ struct FloatVect3 a;
+ FLOAT_QUAT_VMULT(a, q_rp, xaxis);
+
+ // desired heading vect in earth x-y plane
+ struct FloatVect3 psi_vect;
+ psi_vect.x = cosf(stab_att_sp_euler.psi);
+ psi_vect.y = sinf(stab_att_sp_euler.psi);
+ psi_vect.z = 0.0;
+ // normal is the direction of the thrust vector
+ struct FloatVect3 normal;
+ FLOAT_QUAT_VMULT(normal, q_rp, zaxis);
+
+ // projection of desired heading onto body x-y plane
+ // b = v - dot(v,n)*n
+ float dot = FLOAT_VECT3_DOT_PRODUCT(psi_vect, normal);
+ struct FloatVect3 dotn;
+ FLOAT_VECT3_SMUL(dotn, normal, dot);
+
+ // b = v - dot(v,n)*n
+ struct FloatVect3 b;
+ FLOAT_VECT3_DIFF(b, psi_vect, dotn);
+ dot = FLOAT_VECT3_DOT_PRODUCT(a, b);
+ struct FloatVect3 cross;
+ VECT3_CROSS_PRODUCT(cross, a, b);
+ // norm of the cross product
+ float nc = FLOAT_VECT3_NORM(cross);
+ // angle = atan2(norm(cross(a,b)), dot(a,b))
+ float yaw2 = atan2(nc, dot) / 2.0;
+
+ // negative angle if needed
+ // sign(dot(cross(a,b), n)
+ float dot_cross_ab = FLOAT_VECT3_DOT_PRODUCT(cross, normal);
+ if (dot_cross_ab < 0) {
+ yaw2 = -yaw2;
+ }
+
+ /* quaternion with yaw command */
+ struct FloatQuat q_yaw;
+ QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2));
+
+ /* final setpoint: apply roll/pitch, then yaw around resulting body z-axis */
+ FLOAT_QUAT_COMP(stab_att_sp_quat, q_yaw, q_rp);
+ FLOAT_QUAT_NORMALIZE(stab_att_sp_quat);
FLOAT_QUAT_WRAP_SHORTEST(stab_att_sp_quat);
}
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
index b9842627dc..cc64b8567e 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
@@ -137,11 +137,85 @@ void stabilization_attitude_set_failsafe_setpoint(void) {
PPRZ_ITRIG_SIN(stab_att_sp_quat.qz, heading2);
}
-void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler) {
+void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) {
// copy euler setpoint for debugging
- memcpy(&stab_att_sp_euler, sp_euler, sizeof(struct Int32Eulers));
- INT32_QUAT_OF_EULERS(stab_att_sp_quat, *sp_euler);
- INT32_QUAT_WRAP_SHORTEST(stab_att_sp_quat);
+ memcpy(&stab_att_sp_euler, sp_cmd, sizeof(struct Int32Eulers));
+
+ /// @todo calc sp_quat in fixed-point
+
+ /* orientation vector describing simultaneous rotation of roll/pitch */
+ struct FloatVect3 ov;
+ ov.x = ANGLE_FLOAT_OF_BFP(sp_cmd->phi);
+ ov.y = ANGLE_FLOAT_OF_BFP(sp_cmd->theta);
+ ov.z = 0.0;
+ /* quaternion from that orientation vector */
+ struct FloatQuat q_rp;
+ FLOAT_QUAT_OF_ORIENTATION_VECT(q_rp, ov);
+
+ const float psi_sp = ANGLE_FLOAT_OF_BFP(sp_cmd->psi);
+
+ /// @todo optimize yaw angle calculation
+
+ /*
+ * Instead of using the psi setpoint angle to rotate around the body z-axis,
+ * calculate the real angle needed to align the projection of the body x-axis
+ * onto the horizontal plane with the psi setpoint.
+ *
+ * angle between two vectors a and b:
+ * angle = atan2(norm(cross(a,b)), dot(a,b)) * sign(dot(cross(a,b), n))
+ * where n is the thrust vector (i.e. both a and b lie in that plane)
+ */
+ const struct FloatVect3 xaxis = {1.0, 0.0, 0.0};
+ const struct FloatVect3 zaxis = {0.0, 0.0, 1.0};
+ struct FloatVect3 a;
+ FLOAT_QUAT_VMULT(a, q_rp, xaxis);
+
+ // desired heading vect in earth x-y plane
+ struct FloatVect3 psi_vect;
+ psi_vect.x = cosf(psi_sp);
+ psi_vect.y = sinf(psi_sp);
+ psi_vect.z = 0.0;
+ // normal is the direction of the thrust vector
+ struct FloatVect3 normal;
+ FLOAT_QUAT_VMULT(normal, q_rp, zaxis);
+
+ // projection of desired heading onto body x-y plane
+ // b = v - dot(v,n)*n
+ float dot = FLOAT_VECT3_DOT_PRODUCT(psi_vect, normal);
+ struct FloatVect3 dotn;
+ FLOAT_VECT3_SMUL(dotn, normal, dot);
+
+ // b = v - dot(v,n)*n
+ struct FloatVect3 b;
+ FLOAT_VECT3_DIFF(b, psi_vect, dotn);
+
+ dot = FLOAT_VECT3_DOT_PRODUCT(a, b);
+ struct FloatVect3 cross;
+ VECT3_CROSS_PRODUCT(cross, a, b);
+ // norm of the cross product
+ float nc = FLOAT_VECT3_NORM(cross);
+ // angle = atan2(norm(cross(a,b)), dot(a,b))
+ float yaw2 = atan2(nc, dot) / 2.0;
+
+ // negative angle if needed
+ // sign(dot(cross(a,b), n)
+ float dot_cross_ab = FLOAT_VECT3_DOT_PRODUCT(cross, normal);
+ if (dot_cross_ab < 0) {
+ yaw2 = -yaw2;
+ }
+
+ /* quaternion with yaw command */
+ struct FloatQuat q_yaw;
+ QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2));
+
+ /* final setpoint: apply roll/pitch, then yaw around resulting body z-axis */
+ struct FloatQuat q_sp;
+ FLOAT_QUAT_COMP(q_sp, q_yaw, q_rp);
+ FLOAT_QUAT_NORMALIZE(q_sp);
+ FLOAT_QUAT_WRAP_SHORTEST(q_sp);
+
+ /* convert to fixed point */
+ QUAT_BFP_OF_REAL(stab_att_sp_quat, q_sp);
}
#define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
@@ -197,9 +271,13 @@ void stabilization_attitude_run(bool_t enable_integrator) {
INT32_QUAT_NORMALIZE(att_err);
/* rate error */
+ const struct Int32Rates rate_ref_scaled = {
+ OFFSET_AND_ROUND(stab_att_ref_rate.p, (REF_RATE_FRAC - INT32_RATE_FRAC)),
+ OFFSET_AND_ROUND(stab_att_ref_rate.q, (REF_RATE_FRAC - INT32_RATE_FRAC)),
+ OFFSET_AND_ROUND(stab_att_ref_rate.r, (REF_RATE_FRAC - INT32_RATE_FRAC)) };
struct Int32Rates rate_err;
struct Int32Rates* body_rate = stateGetBodyRates_i();
- RATES_DIFF(rate_err, stab_att_ref_rate, *body_rate);
+ RATES_DIFF(rate_err, rate_ref_scaled, (*body_rate));
/* integrated error */
if (enable_integrator) {
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h
similarity index 92%
rename from sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler.h
rename to sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h
index b5076ff7cf..11d41dfb39 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h
@@ -19,12 +19,12 @@
* Boston, MA 02111-1307, USA.
*/
-/** @file firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler.h
- * Common rotorcraft attitude euler reference generation include.
+/** @file firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h
+ * Common rotorcraft attitude reference generation include.
*/
-#ifndef STABILIZATION_ATTITUDE_REF_EULER_H
-#define STABILIZATION_ATTITUDE_REF_EULER_H
+#ifndef STABILIZATION_ATTITUDE_REF_H
+#define STABILIZATION_ATTITUDE_REF_H
#define SATURATE_SPEED_TRIM_ACCEL() { \
if (stab_att_ref_rate.p >= REF_RATE_MAX_P) { \
@@ -60,4 +60,4 @@
}
-#endif /* STABILIZATION_ATTITUDE_REF_EULER_H */
+#endif /* STABILIZATION_ATTITUDE_REF_H */
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h
index fbb74093de..762e66ed96 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h
@@ -25,7 +25,7 @@
#include "math/pprz_algebra_float.h"
#include "stabilization_attitude_ref_float.h"
-#include "stabilization_attitude_ref_euler.h"
+#include "stabilization_attitude_ref.h"
void stabilization_attitude_ref_enter(void);
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c
index 60d24517b8..d7e8566a8b 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c
@@ -74,6 +74,14 @@ void stabilization_attitude_ref_init(void) {
#define OMEGA_2_R BFP_OF_REAL((OMEGA_R*OMEGA_R), OMEGA_2_R_RES)
+#define REF_ANGLE_PI BFP_OF_REAL(3.1415926535897932384626433832795029, REF_ANGLE_FRAC)
+#define REF_ANGLE_TWO_PI BFP_OF_REAL(2.*3.1415926535897932384626433832795029, REF_ANGLE_FRAC)
+#define ANGLE_REF_NORMALIZE(_a) { \
+ while (_a > REF_ANGLE_PI) _a -= REF_ANGLE_TWO_PI; \
+ while (_a < -REF_ANGLE_PI) _a += REF_ANGLE_TWO_PI; \
+ }
+
+
/** explicitly define to zero to disable attitude reference generation */
#ifndef USE_ATTITUDE_REF
#define USE_ATTITUDE_REF 1
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h
index e4dee67c49..bf3aacf19c 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h
@@ -28,7 +28,7 @@
#define STABILIZATION_ATTITUDE_REF_EULER_INT_H
#include "stabilization_attitude_ref_int.h"
-#include "stabilization_attitude_ref_euler.h"
+#include "stabilization_attitude_ref.h"
#endif /* STABILIZATION_ATTITUDE_REF_EULER_INT_H */
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h
index cbaf015206..c0ef65a2b8 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h
@@ -47,13 +47,6 @@ extern struct Int32RefModel stab_att_ref_model;
#define REF_RATE_FRAC 16
#define REF_ANGLE_FRAC 20
-#define REF_ANGLE_PI BFP_OF_REAL(3.1415926535897932384626433832795029, REF_ANGLE_FRAC)
-#define REF_ANGLE_TWO_PI BFP_OF_REAL(2.*3.1415926535897932384626433832795029, REF_ANGLE_FRAC)
-#define ANGLE_REF_NORMALIZE(_a) { \
- while (_a > REF_ANGLE_PI) _a -= REF_ANGLE_TWO_PI; \
- while (_a < -REF_ANGLE_PI) _a += REF_ANGLE_TWO_PI; \
- }
-
extern void stabilization_attitude_ref_init(void);
extern void stabilization_attitude_ref_update(void);
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 8657e30446..da50880f15 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
@@ -37,6 +37,10 @@
#define REF_ACCEL_MAX_Q STABILIZATION_ATTITUDE_REF_MAX_QDOT
#define REF_ACCEL_MAX_R STABILIZATION_ATTITUDE_REF_MAX_RDOT
+#define REF_RATE_MAX_P STABILIZATION_ATTITUDE_REF_MAX_P
+#define REF_RATE_MAX_Q STABILIZATION_ATTITUDE_REF_MAX_Q
+#define REF_RATE_MAX_R STABILIZATION_ATTITUDE_REF_MAX_R
+
struct FloatEulers stab_att_sp_euler;
struct FloatQuat stab_att_sp_quat;
struct FloatEulers stab_att_ref_euler;
@@ -146,6 +150,9 @@ void stabilization_attitude_ref_update(void) {
const struct FloatRates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R };
RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL);
+ /* saturate angular speed and trim accel accordingly */
+ SATURATE_SPEED_TRIM_ACCEL();
+
/* compute ref_euler */
FLOAT_EULERS_OF_QUAT(stab_att_ref_euler, stab_att_ref_quat);
}
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h
index 83323d06dd..0424fbf820 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h
@@ -34,6 +34,7 @@
#include "math/pprz_algebra_float.h"
#include "stabilization_attitude_ref_float.h"
+#include "stabilization_attitude_ref.h"
#define DEADBAND_EXCEEDED(VARIABLE, VALUE) ((VARIABLE > VALUE) || (VARIABLE < -VALUE))
#define APPLY_DEADBAND(VARIABLE, VALUE) (DEADBAND_EXCEEDED(VARIABLE, VALUE) ? VARIABLE : 0.0)
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c
index 52d0a131a6..b0202847dd 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c
@@ -122,11 +122,18 @@ void stabilization_attitude_ref_enter(void)
// which is equal to >> 9
#define F_UPDATE_RES 9
+#define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
+
+
void stabilization_attitude_ref_update(void) {
/* integrate reference attitude */
+ const struct Int32Rates rate_ref_scaled = {
+ OFFSET_AND_ROUND(stab_att_ref_rate.p, (REF_RATE_FRAC - INT32_RATE_FRAC)),
+ OFFSET_AND_ROUND(stab_att_ref_rate.q, (REF_RATE_FRAC - INT32_RATE_FRAC)),
+ OFFSET_AND_ROUND(stab_att_ref_rate.r, (REF_RATE_FRAC - INT32_RATE_FRAC)) };
struct Int32Quat qdot;
- INT32_QUAT_DERIVATIVE(qdot, stab_att_ref_rate, stab_att_ref_quat);
+ INT32_QUAT_DERIVATIVE(qdot, rate_ref_scaled, stab_att_ref_quat);
//QUAT_SMUL(qdot, qdot, DT_UPDATE);
qdot.qi = qdot.qi >> F_UPDATE_RES;
qdot.qx = qdot.qx >> F_UPDATE_RES;
@@ -143,7 +150,6 @@ void stabilization_attitude_ref_update(void) {
stab_att_ref_accel.p >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC),
stab_att_ref_accel.q >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC),
stab_att_ref_accel.r >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC)};
-
RATES_ADD(stab_att_ref_rate, delta_rate);
/* compute reference angular accelerations */
@@ -160,17 +166,20 @@ void stabilization_attitude_ref_update(void) {
((int32_t)(-2.*ZETA_OMEGA_R) * (stab_att_ref_rate.r >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) >> (ZETA_OMEGA_R_RES) };
const struct Int32Rates accel_angle = {
- ((int32_t)(-OMEGA_2_P)* (err.qx >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_P_RES),
- ((int32_t)(-OMEGA_2_Q)* (err.qy >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_Q_RES),
- ((int32_t)(-OMEGA_2_R)* (err.qz >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_R_RES) };
+ ((int32_t)(-OMEGA_2_P) * (err.qx >> (INT32_QUAT_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_P_RES),
+ ((int32_t)(-OMEGA_2_Q) * (err.qy >> (INT32_QUAT_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_Q_RES),
+ ((int32_t)(-OMEGA_2_R) * (err.qz >> (INT32_QUAT_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_R_RES) };
RATES_SUM(stab_att_ref_accel, accel_rate, accel_angle);
- /* saturate acceleration */
- //const struct Int32Rates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R };
- //const struct Int32Rates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R };
- //RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL);
+ /* saturate acceleration */
+ const struct Int32Rates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R };
+ const struct Int32Rates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R };
+ RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL);
+
+ /* saturate angular speed and trim accel accordingly */
+ SATURATE_SPEED_TRIM_ACCEL();
/* compute ref_euler for debugging and telemetry */
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h
index 4fe03f3218..3b1cbaa3e8 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h
@@ -31,6 +31,7 @@
#define STABILIZATION_ATTITUDE_INT_REF_QUAT_INT_H
#include "stabilization_attitude_ref_int.h"
+#include "stabilization_attitude_ref.h"
void stabilization_attitude_ref_enter(void);
diff --git a/sw/airborne/lisa/test/lisa_test_hmc5843.c b/sw/airborne/lisa/test/lisa_test_hmc5843.c
index 5f64d89539..bb76fe423f 100644
--- a/sw/airborne/lisa/test/lisa_test_hmc5843.c
+++ b/sw/airborne/lisa/test/lisa_test_hmc5843.c
@@ -85,6 +85,7 @@ static inline void main_periodic_task( void ) {
});
RunOnceEvery(256,
{
+ uint16_t i2c2_queue_full_cnt = i2c2.errors->queue_full_cnt;
uint16_t i2c2_ack_fail_cnt = i2c2.errors->ack_fail_cnt;
uint16_t i2c2_miss_start_stop_cnt = i2c2.errors->miss_start_stop_cnt;
uint16_t i2c2_arb_lost_cnt = i2c2.errors->arb_lost_cnt;
@@ -96,6 +97,7 @@ static inline void main_periodic_task( void ) {
uint32_t i2c2_last_unexpected_event = i2c2.errors->last_unexpected_event;
const uint8_t _bus2 = 2;
DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice,
+ &i2c2_queue_full_cnt,
&i2c2_ack_fail_cnt,
&i2c2_miss_start_stop_cnt,
&i2c2_arb_lost_cnt,
diff --git a/sw/airborne/lisa/test/lisa_test_itg3200.c b/sw/airborne/lisa/test/lisa_test_itg3200.c
index 81ba2f3f4d..ae7b8583f2 100644
--- a/sw/airborne/lisa/test/lisa_test_itg3200.c
+++ b/sw/airborne/lisa/test/lisa_test_itg3200.c
@@ -81,6 +81,7 @@ static inline void main_periodic_task( void ) {
LED_PERIODIC();
});
RunOnceEvery(256, {
+ uint16_t i2c2_queue_full_cnt = i2c2.errors->queue_full_cnt;
uint16_t i2c2_ack_fail_cnt = i2c2.errors->ack_fail_cnt;
uint16_t i2c2_miss_start_stop_cnt = i2c2.errors->miss_start_stop_cnt;
uint16_t i2c2_arb_lost_cnt = i2c2.errors->arb_lost_cnt;
@@ -92,6 +93,7 @@ static inline void main_periodic_task( void ) {
uint32_t i2c2_last_unexpected_event = i2c2.errors->last_unexpected_event;
const uint8_t _bus2 = 2;
DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice,
+ &i2c2_queue_full_cnt,
&i2c2_ack_fail_cnt,
&i2c2_miss_start_stop_cnt,
&i2c2_arb_lost_cnt,
diff --git a/sw/airborne/lisa/test/test_board.c b/sw/airborne/lisa/test/test_board.c
index a105d61306..432d145d74 100644
--- a/sw/airborne/lisa/test/test_board.c
+++ b/sw/airborne/lisa/test/test_board.c
@@ -155,6 +155,7 @@ static void test_baro_start(void) {all_led_green();}
static void test_baro_periodic(void) {
RunOnceEvery(2, {baro_periodic();});
RunOnceEvery(100,{
+ uint16_t i2c2_queue_full_cnt = i2c2.errors->queue_full_cnt;
uint16_t i2c2_ack_fail_cnt = i2c2.errors->ack_fail_cnt;
uint16_t i2c2_miss_start_stop_cnt = i2c2.errors->miss_start_stop_cnt;
uint16_t i2c2_arb_lost_cnt = i2c2.errors->arb_lost_cnt;
@@ -166,6 +167,7 @@ static void test_baro_periodic(void) {
uint32_t i2c2_last_unexpected_event = i2c2.errors->last_unexpected_event;
const uint8_t _bus2 = 2;
DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice,
+ &i2c2_queue_full_cnt,
&i2c2_ack_fail_cnt,
&i2c2_miss_start_stop_cnt,
&i2c2_arb_lost_cnt,
@@ -200,6 +202,7 @@ static void test_bldc_periodic(void) {
i2c1_transmit(0x58, 1, NULL);
RunOnceEvery(100,{
+ uint16_t i2c1_queue_full_cnt = i2c1.errors->queue_full_cnt;
uint16_t i2c1_ack_fail_cnt = i2c1.errors->ack_fail_cnt;
uint16_t i2c1_miss_start_stop_cnt = i2c1.errors->miss_start_stop_cnt;
uint16_t i2c1_arb_lost_cnt = i2c1.errors->arb_lost_cnt;
@@ -211,6 +214,7 @@ static void test_bldc_periodic(void) {
uint32_t i2c1_last_unexpected_event = i2c1.errors->last_unexpected_event;
const uint8_t _bus1 = 1;
DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice,
+ &i2c1_queue_full_cnt,
&i2c1_ack_fail_cnt,
&i2c1_miss_start_stop_cnt,
&i2c1_arb_lost_cnt,
diff --git a/sw/airborne/lisa/test/test_mc_asctec_v1_simple.c b/sw/airborne/lisa/test/test_mc_asctec_v1_simple.c
index 0579315927..519656f62e 100644
--- a/sw/airborne/lisa/test/test_mc_asctec_v1_simple.c
+++ b/sw/airborne/lisa/test/test_mc_asctec_v1_simple.c
@@ -62,6 +62,7 @@ static inline void main_periodic_task( void ) {
RunOnceEvery(256, {DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);});
RunOnceEvery(256,
{
+ uint16_t i2c1_queue_full_cnt = i2c1.errors->queue_full_cnt;
uint16_t i2c1_ack_fail_cnt = i2c1.errors->ack_fail_cnt;
uint16_t i2c1_miss_start_stop_cnt = i2c1.errors->miss_start_stop_cnt;
uint16_t i2c1_arb_lost_cnt = i2c1.errors->arb_lost_cnt;
@@ -73,6 +74,7 @@ static inline void main_periodic_task( void ) {
uint32_t i2c1_last_unexpected_event = i2c1.errors->last_unexpected_event;
const uint8_t _bus1 = 1;
DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice,
+ &i2c1_queue_full_cnt,
&i2c1_ack_fail_cnt,
&i2c1_miss_start_stop_cnt,
&i2c1_arb_lost_cnt,
diff --git a/sw/airborne/lisa/test_baro_i2c.c b/sw/airborne/lisa/test_baro_i2c.c
index 1a63ad6274..250bf89c33 100644
--- a/sw/airborne/lisa/test_baro_i2c.c
+++ b/sw/airborne/lisa/test_baro_i2c.c
@@ -78,6 +78,7 @@ static inline void main_periodic_task( void ) {
RunOnceEvery(256, {DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);});
RunOnceEvery(256,
{
+ uint16_t i2c2_queue_full_cnt = i2c2.errors->queue_full_cnt;
uint16_t i2c2_ack_fail_cnt = i2c2.errors->ack_fail_cnt;
uint16_t i2c2_miss_start_stop_cnt = i2c2.errors->miss_start_stop_cnt;
uint16_t i2c2_arb_lost_cnt = i2c2.errors->arb_lost_cnt;
@@ -89,6 +90,7 @@ static inline void main_periodic_task( void ) {
uint32_t i2c2_last_unexpected_event = i2c2.errors->last_unexpected_event;
const uint8_t _bus2 = 2;
DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice,
+ &i2c2_queue_full_cnt,
&i2c2_ack_fail_cnt,
&i2c2_miss_start_stop_cnt,
&i2c2_arb_lost_cnt,
diff --git a/sw/airborne/lisa/tunnel_hw.c b/sw/airborne/lisa/tunnel_hw.c
index 9f624f7f0b..510a48314f 100644
--- a/sw/airborne/lisa/tunnel_hw.c
+++ b/sw/airborne/lisa/tunnel_hw.c
@@ -27,6 +27,7 @@
#include "mcu_periph/sys_time.h"
#include "led.h"
+/* UART1 */
#define A_PERIPH RCC_APB2ENR_IOPAEN
#define A_PORT GPIOA
#define A_RX_PIN GPIO10
@@ -34,6 +35,7 @@
#define A_TX_PIN GPIO9
#define A_TX_PORT A_PORT
+/* UART2 */
#define B_PERIPH RCC_APB2ENR_IOPAEN
#define B_PORT GPIOA
#define B_RX_PIN GPIO3
diff --git a/sw/airborne/math/pprz_algebra.h b/sw/airborne/math/pprz_algebra.h
index 4bf69fc197..25439b28c5 100644
--- a/sw/airborne/math/pprz_algebra.h
+++ b/sw/airborne/math/pprz_algebra.h
@@ -626,6 +626,18 @@
(_ri).r = RATE_BFP_OF_REAL((_rf).r); \
}
+#define POSITIONS_FLOAT_OF_BFP(_ef, _ei) { \
+ (_ef).x = POS_FLOAT_OF_BFP((_ei).x); \
+ (_ef).y = POS_FLOAT_OF_BFP((_ei).y); \
+ (_ef).z = POS_FLOAT_OF_BFP((_ei).z); \
+ }
+
+#define POSITIONS_BFP_OF_REAL(_ef, _ei) { \
+ (_ef).x = POS_BFP_OF_REAL((_ei).x); \
+ (_ef).y = POS_BFP_OF_REAL((_ei).y); \
+ (_ef).z = POS_BFP_OF_REAL((_ei).z); \
+ }
+
#define SPEEDS_FLOAT_OF_BFP(_ef, _ei) { \
(_ef).x = SPEED_FLOAT_OF_BFP((_ei).x); \
(_ef).y = SPEED_FLOAT_OF_BFP((_ei).y); \
diff --git a/sw/airborne/math/pprz_algebra_double.h b/sw/airborne/math/pprz_algebra_double.h
index cdcc260162..4a5bdda494 100644
--- a/sw/airborne/math/pprz_algebra_double.h
+++ b/sw/airborne/math/pprz_algebra_double.h
@@ -86,10 +86,10 @@ struct DoubleRates {
#define DOUBLE_VECT3_ROUND(_v) DOUBLE_VECT3_RINT(_v, _v)
-#define DOUBLE_VECT3_RINT(_vout, _vin) { \
- (_vout).x = rint((_vin).x); \
- (_vout).y = rint((_vin).y); \
- (_vout).z = rint((_vin).z); \
+#define DOUBLE_VECT3_RINT(_vout, _vin) { \
+ (_vout).x = rint((_vin).x); \
+ (_vout).y = rint((_vin).y); \
+ (_vout).z = rint((_vin).z); \
}
#define DOUBLE_VECT3_ASSIGN(_a, _x, _y, _z) VECT3_ASSIGN(_a, _x, _y, _z)
@@ -97,94 +97,94 @@ struct DoubleRates {
#define DOUBLE_VECT3_COPY(_a, _b) VECT3_COPY(_a, _b)
#define DOUBLE_VECT3_SUM(_c,_a,_b) { \
- (_c).x = (_a).x + (_b).x; \
- (_c).y = (_a).y + (_b).y; \
- (_c).z = (_a).z + (_b).z; \
+ (_c).x = (_a).x + (_b).x; \
+ (_c).y = (_a).y + (_b).y; \
+ (_c).z = (_a).z + (_b).z; \
}
#define DOUBLE_VECT3_CROSS_PRODUCT(vo, v1, v2) FLOAT_VECT3_CROSS_PRODUCT(vo, v1, v2)
#define DOUBLE_RMAT_OF_EULERS(_rm, _e) DOUBLE_RMAT_OF_EULERS_321(_rm, _e)
-#define DOUBLE_RMAT_OF_EULERS_321(_rm, _e) { \
- \
- const double sphi = sin((_e).phi); \
- const double cphi = cos((_e).phi); \
- const double stheta = sin((_e).theta); \
- const double ctheta = cos((_e).theta); \
- const double spsi = sin((_e).psi); \
- const double cpsi = cos((_e).psi); \
- \
- RMAT_ELMT(_rm, 0, 0) = ctheta*cpsi; \
- RMAT_ELMT(_rm, 0, 1) = ctheta*spsi; \
- RMAT_ELMT(_rm, 0, 2) = -stheta; \
- RMAT_ELMT(_rm, 1, 0) = sphi*stheta*cpsi - cphi*spsi; \
- RMAT_ELMT(_rm, 1, 1) = sphi*stheta*spsi + cphi*cpsi; \
- RMAT_ELMT(_rm, 1, 2) = sphi*ctheta; \
- RMAT_ELMT(_rm, 2, 0) = cphi*stheta*cpsi + sphi*spsi; \
- RMAT_ELMT(_rm, 2, 1) = cphi*stheta*spsi - sphi*cpsi; \
- RMAT_ELMT(_rm, 2, 2) = cphi*ctheta; \
- \
+#define DOUBLE_RMAT_OF_EULERS_321(_rm, _e) { \
+ \
+ const double sphi = sin((_e).phi); \
+ const double cphi = cos((_e).phi); \
+ const double stheta = sin((_e).theta); \
+ const double ctheta = cos((_e).theta); \
+ const double spsi = sin((_e).psi); \
+ const double cpsi = cos((_e).psi); \
+ \
+ RMAT_ELMT(_rm, 0, 0) = ctheta*cpsi; \
+ RMAT_ELMT(_rm, 0, 1) = ctheta*spsi; \
+ RMAT_ELMT(_rm, 0, 2) = -stheta; \
+ RMAT_ELMT(_rm, 1, 0) = sphi*stheta*cpsi - cphi*spsi; \
+ RMAT_ELMT(_rm, 1, 1) = sphi*stheta*spsi + cphi*cpsi; \
+ RMAT_ELMT(_rm, 1, 2) = sphi*ctheta; \
+ RMAT_ELMT(_rm, 2, 0) = cphi*stheta*cpsi + sphi*spsi; \
+ RMAT_ELMT(_rm, 2, 1) = cphi*stheta*spsi - sphi*cpsi; \
+ RMAT_ELMT(_rm, 2, 2) = cphi*ctheta; \
+ \
}
/* multiply _vin by _mat, store in _vout */
-#define DOUBLE_MAT33_VECT3_MUL(_vout, _mat, _vin) { \
- (_vout).x = (_mat)[0]*(_vin).x + (_mat)[1]*(_vin).y + (_mat)[2]*(_vin).z; \
- (_vout).y = (_mat)[3]*(_vin).x + (_mat)[4]*(_vin).y + (_mat)[5]*(_vin).z; \
- (_vout).z = (_mat)[6]*(_vin).x + (_mat)[7]*(_vin).y + (_mat)[8]*(_vin).z; \
+#define DOUBLE_MAT33_VECT3_MUL(_vout, _mat, _vin) { \
+ (_vout).x = (_mat)[0]*(_vin).x + (_mat)[1]*(_vin).y + (_mat)[2]*(_vin).z; \
+ (_vout).y = (_mat)[3]*(_vin).x + (_mat)[4]*(_vin).y + (_mat)[5]*(_vin).z; \
+ (_vout).z = (_mat)[6]*(_vin).x + (_mat)[7]*(_vin).y + (_mat)[8]*(_vin).z; \
}
/* multiply _vin by the transpose of _mat, store in _vout */
-#define DOUBLE_MAT33_VECT3_TRANSP_MUL(_vout, _mat, _vin) { \
- (_vout).x = (_mat)[0]*(_vin).x + (_mat)[3]*(_vin).y + (_mat)[6]*(_vin).z; \
- (_vout).y = (_mat)[1]*(_vin).x + (_mat)[4]*(_vin).y + (_mat)[7]*(_vin).z; \
- (_vout).z = (_mat)[2]*(_vin).x + (_mat)[5]*(_vin).y + (_mat)[8]*(_vin).z; \
+#define DOUBLE_MAT33_VECT3_TRANSP_MUL(_vout, _mat, _vin) { \
+ (_vout).x = (_mat)[0]*(_vin).x + (_mat)[3]*(_vin).y + (_mat)[6]*(_vin).z; \
+ (_vout).y = (_mat)[1]*(_vin).x + (_mat)[4]*(_vin).y + (_mat)[7]*(_vin).z; \
+ (_vout).z = (_mat)[2]*(_vin).x + (_mat)[5]*(_vin).y + (_mat)[8]*(_vin).z; \
}
-#define DOUBLE_QUAT_OF_EULERS(_q, _e) { \
- \
- const double phi2 = (_e).phi/ 2.0; \
- const double theta2 = (_e).theta/2.0; \
- const double psi2 = (_e).psi/2.0; \
- \
- const double s_phi2 = sin( phi2 ); \
- const double c_phi2 = cos( phi2 ); \
- const double s_theta2 = sin( theta2 ); \
- const double c_theta2 = cos( theta2 ); \
- const double s_psi2 = sin( psi2 ); \
- const double c_psi2 = cos( psi2 ); \
- \
+#define DOUBLE_QUAT_OF_EULERS(_q, _e) { \
+ \
+ const double phi2 = (_e).phi/ 2.0; \
+ const double theta2 = (_e).theta/2.0; \
+ const double psi2 = (_e).psi/2.0; \
+ \
+ const double s_phi2 = sin( phi2 ); \
+ const double c_phi2 = cos( phi2 ); \
+ const double s_theta2 = sin( theta2 ); \
+ const double c_theta2 = cos( theta2 ); \
+ const double s_psi2 = sin( psi2 ); \
+ const double c_psi2 = cos( psi2 ); \
+ \
(_q).qi = c_phi2 * c_theta2 * c_psi2 + s_phi2 * s_theta2 * s_psi2; \
(_q).qx = -c_phi2 * s_theta2 * s_psi2 + s_phi2 * c_theta2 * c_psi2; \
(_q).qy = c_phi2 * s_theta2 * c_psi2 + s_phi2 * c_theta2 * s_psi2; \
(_q).qz = c_phi2 * c_theta2 * s_psi2 - s_phi2 * s_theta2 * c_psi2; \
- \
+ \
}
-#define DOUBLE_EULERS_OF_QUAT(_e, _q) { \
- \
- const double qx2 = (_q).qx*(_q).qx; \
- const double qy2 = (_q).qy*(_q).qy; \
- const double qz2 = (_q).qz*(_q).qz; \
- const double qiqx = (_q).qi*(_q).qx; \
- const double qiqy = (_q).qi*(_q).qy; \
- const double qiqz = (_q).qi*(_q).qz; \
- const double qxqy = (_q).qx*(_q).qy; \
- const double qxqz = (_q).qx*(_q).qz; \
- const double qyqz = (_q).qy*(_q).qz; \
- const double dcm00 = 1.0 - 2.*( qy2 + qz2 ); \
- const double dcm01 = 2.*( qxqy + qiqz ); \
- const double dcm02 = 2.*( qxqz - qiqy ); \
- const double dcm12 = 2.*( qyqz + qiqx ); \
- const double dcm22 = 1.0 - 2.*( qx2 + qy2 ); \
- \
- (_e).phi = atan2( dcm12, dcm22 ); \
- (_e).theta = -asin( dcm02 ); \
- (_e).psi = atan2( dcm01, dcm00 ); \
- \
+#define DOUBLE_EULERS_OF_QUAT(_e, _q) { \
+ \
+ const double qx2 = (_q).qx*(_q).qx; \
+ const double qy2 = (_q).qy*(_q).qy; \
+ const double qz2 = (_q).qz*(_q).qz; \
+ const double qiqx = (_q).qi*(_q).qx; \
+ const double qiqy = (_q).qi*(_q).qy; \
+ const double qiqz = (_q).qi*(_q).qz; \
+ const double qxqy = (_q).qx*(_q).qy; \
+ const double qxqz = (_q).qx*(_q).qz; \
+ const double qyqz = (_q).qy*(_q).qz; \
+ const double dcm00 = 1.0 - 2.*( qy2 + qz2 ); \
+ const double dcm01 = 2.*( qxqy + qiqz ); \
+ const double dcm02 = 2.*( qxqz - qiqy ); \
+ const double dcm12 = 2.*( qyqz + qiqx ); \
+ const double dcm22 = 1.0 - 2.*( qx2 + qy2 ); \
+ \
+ (_e).phi = atan2( dcm12, dcm22 ); \
+ (_e).theta = -asin( dcm02 ); \
+ (_e).psi = atan2( dcm01, dcm00 ); \
+ \
}
#endif /* PPRZ_ALGEBRA_DOUBLE_H */
diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h
index e48aca10dd..799087f1f6 100644
--- a/sw/airborne/math/pprz_algebra_float.h
+++ b/sw/airborne/math/pprz_algebra_float.h
@@ -90,9 +90,9 @@ struct FloatRates {
float r; ///< in rad/s^2
};
-#define FLOAT_ANGLE_NORMALIZE(_a) { \
- while (_a > M_PI) _a -= (2.*M_PI); \
- while (_a < -M_PI) _a += (2.*M_PI); \
+#define FLOAT_ANGLE_NORMALIZE(_a) { \
+ while (_a > M_PI) _a -= (2.*M_PI); \
+ while (_a < -M_PI) _a += (2.*M_PI); \
}
@@ -127,6 +127,11 @@ struct FloatRates {
n = sqrtf((v).x*(v).x + (v).y*(v).y); \
}
+#define FLOAT_VECT2_NORMALIZE(_v) { \
+ const float n = sqrtf((_v).x*(_v).x + (_v).y*(_v).y); \
+ FLOAT_VECT2_SMUL(_v, _v, 1./n); \
+ }
+
/*
* Dimension 3 Vectors
@@ -151,58 +156,58 @@ struct FloatRates {
#define FLOAT_VECT3_DOT_PRODUCT(_v1, _v2) ((_v1).x*(_v2).x + (_v1).y*(_v2).y + (_v1).z*(_v2).z)
-#define FLOAT_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; \
+#define FLOAT_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; \
}
-#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_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); \
+#define FLOAT_VECT3_NORMALIZE(_v) { \
+ const float n = FLOAT_VECT3_NORM(_v); \
+ FLOAT_VECT3_SMUL(_v, _v, 1./n); \
}
-#define FLOAT_RATES_ZERO(_r) { \
- RATES_ASSIGN(_r, 0., 0., 0.); \
+#define FLOAT_RATES_ZERO(_r) { \
+ RATES_ASSIGN(_r, 0., 0., 0.); \
}
#define FLOAT_RATES_NORM(_v) (sqrtf((_v).p*(_v).p + (_v).q*(_v).q + (_v).r*(_v).r))
-#define FLOAT_RATES_ADD_SCALED_VECT(_ro, _v, _s) { \
- _ro.p += _v.x * _s; \
- _ro.q += _v.y * _s; \
- _ro.r += _v.z * _s; \
+#define FLOAT_RATES_ADD_SCALED_VECT(_ro, _v, _s) { \
+ _ro.p += _v.x * _s; \
+ _ro.q += _v.y * _s; \
+ _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_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_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_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) { \
+#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; \
@@ -213,28 +218,28 @@ struct FloatRates {
/*
* 3x3 matrices
*/
-#define FLOAT_MAT33_ZERO(_m) { \
- MAT33_ELMT(_m, 0, 0) = 0.; \
- MAT33_ELMT(_m, 0, 1) = 0.; \
+#define FLOAT_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, 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 FLOAT_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; \
+#define FLOAT_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; \
}
@@ -246,48 +251,48 @@ struct FloatRates {
#define FLOAT_RMAT_ZERO(_rm) FLOAT_MAT33_DIAG(_rm, 1., 1., 1.)
/* initialises a rotation matrix from unit vector axis and angle */
-#define FLOAT_RMAT_OF_AXIS_ANGLE(_rm, _uv, _an) { \
- \
- const float ux2 = _uv.x*_uv.x; \
- const float uy2 = _uv.y*_uv.y; \
- const float uz2 = _uv.z*_uv.z; \
- const float uxuy = _uv.x*_uv.y; \
- const float uyuz = _uv.y*_uv.z; \
- const float uxuz = _uv.x*_uv.z; \
- const float can = cosf(_an); \
- const float san = sinf(_an); \
- const float one_m_can = (1. - can); \
- \
- RMAT_ELMT(_rm, 0, 0) = ux2 + (1.-ux2)*can; \
- RMAT_ELMT(_rm, 0, 1) = uxuy*one_m_can + _uv.z*san; \
- RMAT_ELMT(_rm, 0, 2) = uxuz*one_m_can - _uv.y*san; \
- RMAT_ELMT(_rm, 1, 0) = uxuy*one_m_can - _uv.z*san; \
- RMAT_ELMT(_rm, 1, 1) = uy2 + (1.-uy2)*can; \
- RMAT_ELMT(_rm, 1, 2) = uyuz*one_m_can + _uv.x*san; \
- RMAT_ELMT(_rm, 2, 0) = uxuz*one_m_can + _uv.y*san; \
- RMAT_ELMT(_rm, 2, 1) = uyuz*one_m_can - _uv.x*san; \
- RMAT_ELMT(_rm, 2, 2) = uz2 + (1.-uz2)*can; \
- \
+#define FLOAT_RMAT_OF_AXIS_ANGLE(_rm, _uv, _an) { \
+ \
+ const float ux2 = _uv.x*_uv.x; \
+ const float uy2 = _uv.y*_uv.y; \
+ const float uz2 = _uv.z*_uv.z; \
+ const float uxuy = _uv.x*_uv.y; \
+ const float uyuz = _uv.y*_uv.z; \
+ const float uxuz = _uv.x*_uv.z; \
+ const float can = cosf(_an); \
+ const float san = sinf(_an); \
+ const float one_m_can = (1. - can); \
+ \
+ RMAT_ELMT(_rm, 0, 0) = ux2 + (1.-ux2)*can; \
+ RMAT_ELMT(_rm, 0, 1) = uxuy*one_m_can + _uv.z*san; \
+ RMAT_ELMT(_rm, 0, 2) = uxuz*one_m_can - _uv.y*san; \
+ RMAT_ELMT(_rm, 1, 0) = uxuy*one_m_can - _uv.z*san; \
+ RMAT_ELMT(_rm, 1, 1) = uy2 + (1.-uy2)*can; \
+ RMAT_ELMT(_rm, 1, 2) = uyuz*one_m_can + _uv.x*san; \
+ RMAT_ELMT(_rm, 2, 0) = uxuz*one_m_can + _uv.y*san; \
+ RMAT_ELMT(_rm, 2, 1) = uyuz*one_m_can - _uv.x*san; \
+ RMAT_ELMT(_rm, 2, 2) = uz2 + (1.-uz2)*can; \
+ \
}
/* 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) { \
+#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); \
(_vb).q = ( (_m_b2a).m[1]*(_va).p + (_m_b2a).m[4]*(_va).q + (_m_b2a).m[7]*(_va).r); \
(_vb).r = ( (_m_b2a).m[2]*(_va).p + (_m_b2a).m[5]*(_va).q + (_m_b2a).m[8]*(_va).r); \
}
-#define FLOAT_RMAT_RATEMULT(_vb, _m_a2b, _va) { \
+#define FLOAT_RMAT_RATEMULT(_vb, _m_a2b, _va) { \
(_vb).p = ( (_m_a2b).m[0]*(_va).p + (_m_a2b).m[1]*(_va).q + (_m_a2b).m[2]*(_va).r); \
(_vb).q = ( (_m_a2b).m[3]*(_va).p + (_m_a2b).m[4]*(_va).q + (_m_a2b).m[5]*(_va).r); \
(_vb).r = ( (_m_a2b).m[6]*(_va).p + (_m_a2b).m[7]*(_va).q + (_m_a2b).m[8]*(_va).r); \
}
/* _m_a2c = _m_a2b comp _m_b2c , aka _m_a2c = _m_b2c * _m_a2b */
-#define FLOAT_RMAT_COMP(_m_a2c, _m_a2b, _m_b2c) { \
+#define FLOAT_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]); \
_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]); \
_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]); \
@@ -301,7 +306,7 @@ struct FloatRates {
/* _m_a2b = _m_a2c comp_inv _m_b2c , aka _m_a2b = inv(_m_b2c) * _m_a2c */
-#define FLOAT_RMAT_COMP_INV(_m_a2b, _m_a2c, _m_b2c) { \
+#define FLOAT_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]); \
_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]); \
_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]); \
@@ -315,136 +320,104 @@ struct FloatRates {
/* _m_b2a = inv(_m_a2b) = transp(_m_a2b) */
-#define FLOAT_RMAT_INV(_m_b2a, _m_a2b) { \
- RMAT_ELMT(_m_b2a, 0, 0) = RMAT_ELMT(_m_a2b, 0, 0); \
- RMAT_ELMT(_m_b2a, 0, 1) = RMAT_ELMT(_m_a2b, 1, 0); \
- RMAT_ELMT(_m_b2a, 0, 2) = RMAT_ELMT(_m_a2b, 2, 0); \
- RMAT_ELMT(_m_b2a, 1, 0) = RMAT_ELMT(_m_a2b, 0, 1); \
- RMAT_ELMT(_m_b2a, 1, 1) = RMAT_ELMT(_m_a2b, 1, 1); \
- RMAT_ELMT(_m_b2a, 1, 2) = RMAT_ELMT(_m_a2b, 2, 1); \
- RMAT_ELMT(_m_b2a, 2, 0) = RMAT_ELMT(_m_a2b, 0, 2); \
- RMAT_ELMT(_m_b2a, 2, 1) = RMAT_ELMT(_m_a2b, 1, 2); \
- RMAT_ELMT(_m_b2a, 2, 2) = RMAT_ELMT(_m_a2b, 2, 2); \
+#define FLOAT_RMAT_INV(_m_b2a, _m_a2b) { \
+ RMAT_ELMT(_m_b2a, 0, 0) = RMAT_ELMT(_m_a2b, 0, 0); \
+ RMAT_ELMT(_m_b2a, 0, 1) = RMAT_ELMT(_m_a2b, 1, 0); \
+ RMAT_ELMT(_m_b2a, 0, 2) = RMAT_ELMT(_m_a2b, 2, 0); \
+ RMAT_ELMT(_m_b2a, 1, 0) = RMAT_ELMT(_m_a2b, 0, 1); \
+ RMAT_ELMT(_m_b2a, 1, 1) = RMAT_ELMT(_m_a2b, 1, 1); \
+ RMAT_ELMT(_m_b2a, 1, 2) = RMAT_ELMT(_m_a2b, 2, 1); \
+ RMAT_ELMT(_m_b2a, 2, 0) = RMAT_ELMT(_m_a2b, 0, 2); \
+ RMAT_ELMT(_m_b2a, 2, 1) = RMAT_ELMT(_m_a2b, 1, 2); \
+ RMAT_ELMT(_m_b2a, 2, 2) = RMAT_ELMT(_m_a2b, 2, 2); \
}
-#define FLOAT_RMAT_NORM(_m) ( \
- sqrtf(SQUARE((_m).m[0])+ SQUARE((_m).m[1])+ SQUARE((_m).m[2])+ \
- SQUARE((_m).m[3])+ SQUARE((_m).m[4])+ SQUARE((_m).m[5])+ \
- SQUARE((_m).m[6])+ SQUARE((_m).m[7])+ SQUARE((_m).m[8])) \
+#define FLOAT_RMAT_NORM(_m) ( \
+ sqrtf(SQUARE((_m).m[0])+ SQUARE((_m).m[1])+ SQUARE((_m).m[2])+ \
+ SQUARE((_m).m[3])+ SQUARE((_m).m[4])+ SQUARE((_m).m[5])+ \
+ SQUARE((_m).m[6])+ SQUARE((_m).m[7])+ SQUARE((_m).m[8])) \
)
#define FLOAT_RMAT_OF_EULERS(_rm, _e) FLOAT_RMAT_OF_EULERS_321(_rm, _e)
/* C n->b rotation matrix */
-#define FLOAT_RMAT_OF_EULERS_321(_rm, _e) { \
- \
- const float sphi = sinf((_e).phi); \
- const float cphi = cosf((_e).phi); \
- const float stheta = sinf((_e).theta); \
- const float ctheta = cosf((_e).theta); \
- const float spsi = sinf((_e).psi); \
- const float cpsi = cosf((_e).psi); \
- \
- RMAT_ELMT(_rm, 0, 0) = ctheta*cpsi; \
- RMAT_ELMT(_rm, 0, 1) = ctheta*spsi; \
- RMAT_ELMT(_rm, 0, 2) = -stheta; \
- RMAT_ELMT(_rm, 1, 0) = sphi*stheta*cpsi - cphi*spsi; \
- RMAT_ELMT(_rm, 1, 1) = sphi*stheta*spsi + cphi*cpsi; \
- RMAT_ELMT(_rm, 1, 2) = sphi*ctheta; \
- RMAT_ELMT(_rm, 2, 0) = cphi*stheta*cpsi + sphi*spsi; \
- RMAT_ELMT(_rm, 2, 1) = cphi*stheta*spsi - sphi*cpsi; \
- RMAT_ELMT(_rm, 2, 2) = cphi*ctheta; \
- \
+#define FLOAT_RMAT_OF_EULERS_321(_rm, _e) { \
+ \
+ const float sphi = sinf((_e).phi); \
+ const float cphi = cosf((_e).phi); \
+ const float stheta = sinf((_e).theta); \
+ const float ctheta = cosf((_e).theta); \
+ const float spsi = sinf((_e).psi); \
+ const float cpsi = cosf((_e).psi); \
+ \
+ RMAT_ELMT(_rm, 0, 0) = ctheta*cpsi; \
+ RMAT_ELMT(_rm, 0, 1) = ctheta*spsi; \
+ RMAT_ELMT(_rm, 0, 2) = -stheta; \
+ RMAT_ELMT(_rm, 1, 0) = sphi*stheta*cpsi - cphi*spsi; \
+ RMAT_ELMT(_rm, 1, 1) = sphi*stheta*spsi + cphi*cpsi; \
+ RMAT_ELMT(_rm, 1, 2) = sphi*ctheta; \
+ RMAT_ELMT(_rm, 2, 0) = cphi*stheta*cpsi + sphi*spsi; \
+ RMAT_ELMT(_rm, 2, 1) = cphi*stheta*spsi - sphi*cpsi; \
+ RMAT_ELMT(_rm, 2, 2) = cphi*ctheta; \
+ \
}
-#define FLOAT_RMAT_OF_EULERS_312(_rm, _e) { \
- \
- const float sphi = sinf((_e).phi); \
- const float cphi = cosf((_e).phi); \
- const float stheta = sinf((_e).theta); \
- const float ctheta = cosf((_e).theta); \
- const float spsi = sinf((_e).psi); \
- const float cpsi = cosf((_e).psi); \
- \
- RMAT_ELMT(_rm, 0, 0) = ctheta*cpsi - sphi * stheta * spsi; \
- RMAT_ELMT(_rm, 0, 1) = ctheta*spsi + sphi * stheta * cpsi; \
- RMAT_ELMT(_rm, 0, 2) = -cphi * stheta; \
- RMAT_ELMT(_rm, 1, 0) = -cphi * spsi; \
- RMAT_ELMT(_rm, 1, 1) = cphi * cpsi; \
- RMAT_ELMT(_rm, 1, 2) = sphi; \
- RMAT_ELMT(_rm, 2, 0) = stheta*cpsi + sphi*ctheta*spsi; \
- RMAT_ELMT(_rm, 2, 1) = stheta*spsi - sphi*ctheta*cpsi; \
- RMAT_ELMT(_rm, 2, 2) = cphi*ctheta; \
- \
+#define FLOAT_RMAT_OF_EULERS_312(_rm, _e) { \
+ \
+ const float sphi = sinf((_e).phi); \
+ const float cphi = cosf((_e).phi); \
+ const float stheta = sinf((_e).theta); \
+ const float ctheta = cosf((_e).theta); \
+ const float spsi = sinf((_e).psi); \
+ const float cpsi = cosf((_e).psi); \
+ \
+ RMAT_ELMT(_rm, 0, 0) = ctheta*cpsi - sphi * stheta * spsi; \
+ RMAT_ELMT(_rm, 0, 1) = ctheta*spsi + sphi * stheta * cpsi; \
+ RMAT_ELMT(_rm, 0, 2) = -cphi * stheta; \
+ RMAT_ELMT(_rm, 1, 0) = -cphi * spsi; \
+ RMAT_ELMT(_rm, 1, 1) = cphi * cpsi; \
+ RMAT_ELMT(_rm, 1, 2) = sphi; \
+ RMAT_ELMT(_rm, 2, 0) = stheta*cpsi + sphi*ctheta*spsi; \
+ RMAT_ELMT(_rm, 2, 1) = stheta*spsi - sphi*ctheta*cpsi; \
+ RMAT_ELMT(_rm, 2, 2) = cphi*ctheta; \
+ \
}
/* C n->b rotation matrix */
-#ifdef ALGEBRA_FLOAT_USE_SLOW_FUNCTIONS
-#define FLOAT_RMAT_OF_QUAT(_rm, _q) { \
- const float qx2 = (_q).qx*(_q).qx; \
- const float qy2 = (_q).qy*(_q).qy; \
- const float qz2 = (_q).qz*(_q).qz; \
- const float qiqx = (_q).qi*(_q).qx; \
- const float qiqy = (_q).qi*(_q).qy; \
- const float qiqz = (_q).qi*(_q).qz; \
- const float qxqy = (_q).qx*(_q).qy; \
- const float qxqz = (_q).qx*(_q).qz; \
- const float qyqz = (_q).qy*(_q).qz; \
- /* dcm00 = 1.0 - 2.*( qy2 + qz2 ); */ \
- RMAT_ELMT(_rm, 0, 0) = 1. - 2.*( qy2 + qz2 ); \
- /* dcm01 = 2.*( qxqy + qiqz ); */ \
- RMAT_ELMT(_rm, 0, 1) = 2.*( qxqy + qiqz ); \
- /* dcm02 = 2.*( qxqz - qiqy ); */ \
- RMAT_ELMT(_rm, 0, 2) = 2.*( qxqz - qiqy ); \
- /* dcm10 = 2.*( qxqy - qiqz ); */ \
- RMAT_ELMT(_rm, 1, 0) = 2.*( qxqy - qiqz ); \
- /* dcm11 = 1.0 - 2.*(qx2+qz2); */ \
- RMAT_ELMT(_rm, 1, 1) = 1.0 - 2.*(qx2+qz2); \
- /* dcm12 = 2.*( qyqz + qiqx ); */ \
- RMAT_ELMT(_rm, 1, 2) = 2.*( qyqz + qiqx ); \
- /* dcm20 = 2.*( qxqz + qiqy ); */ \
- RMAT_ELMT(_rm, 2, 0) = 2.*( qxqz + qiqy ); \
- /* dcm21 = 2.*( qyqz - qiqx ); */ \
- RMAT_ELMT(_rm, 2, 1) = 2.*( qyqz - qiqx ); \
- /* dcm22 = 1.0 - 2.*( qx2 + qy2 ); */ \
- RMAT_ELMT(_rm, 2, 2) = 1.0 - 2.*( qx2 + qy2 ); \
+#define FLOAT_RMAT_OF_QUAT(_rm, _q) { \
+ const float _a = M_SQRT2*(_q).qi; \
+ const float _b = M_SQRT2*(_q).qx; \
+ const float _c = M_SQRT2*(_q).qy; \
+ const float _d = M_SQRT2*(_q).qz; \
+ const float a2_1 = _a*_a-1; \
+ const float ab = _a*_b; \
+ const float ac = _a*_c; \
+ const float ad = _a*_d; \
+ const float bc = _b*_c; \
+ const float bd = _b*_d; \
+ const float cd = _c*_d; \
+ RMAT_ELMT(_rm, 0, 0) = a2_1+_b*_b; \
+ RMAT_ELMT(_rm, 0, 1) = bc+ad; \
+ RMAT_ELMT(_rm, 0, 2) = bd-ac; \
+ RMAT_ELMT(_rm, 1, 0) = bc-ad; \
+ RMAT_ELMT(_rm, 1, 1) = a2_1+_c*_c; \
+ RMAT_ELMT(_rm, 1, 2) = cd+ab; \
+ RMAT_ELMT(_rm, 2, 0) = bd+ac; \
+ RMAT_ELMT(_rm, 2, 1) = cd-ab; \
+ RMAT_ELMT(_rm, 2, 2) = a2_1+_d*_d; \
}
-#else
-#define FLOAT_RMAT_OF_QUAT(_rm, _q) { \
- const float _a = M_SQRT2*(_q).qi; \
- const float _b = M_SQRT2*(_q).qx; \
- const float _c = M_SQRT2*(_q).qy; \
- const float _d = M_SQRT2*(_q).qz; \
- const float a2_1 = _a*_a-1; \
- const float ab = _a*_b; \
- const float ac = _a*_c; \
- const float ad = _a*_d; \
- const float bc = _b*_c; \
- const float bd = _b*_d; \
- const float cd = _c*_d; \
- RMAT_ELMT(_rm, 0, 0) = a2_1+_b*_b; \
- RMAT_ELMT(_rm, 0, 1) = bc+ad; \
- RMAT_ELMT(_rm, 0, 2) = bd-ac; \
- RMAT_ELMT(_rm, 1, 0) = bc-ad; \
- RMAT_ELMT(_rm, 1, 1) = a2_1+_c*_c; \
- RMAT_ELMT(_rm, 1, 2) = cd+ab; \
- RMAT_ELMT(_rm, 2, 0) = bd+ac; \
- RMAT_ELMT(_rm, 2, 1) = cd-ab; \
- RMAT_ELMT(_rm, 2, 2) = a2_1+_d*_d; \
- }
-#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)); \
+#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)); \
}
@@ -500,10 +473,10 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) {
#define FLOAT_QUAT_COPY(_qo, _qi) QUAT_COPY(_qo, _qi)
-#define FLOAT_QUAT_NORM(_q) (sqrtf(SQUARE((_q).qi) + SQUARE((_q).qx)+ \
+#define FLOAT_QUAT_NORM(_q) (sqrtf(SQUARE((_q).qi) + SQUARE((_q).qx)+ \
SQUARE((_q).qy) + SQUARE((_q).qz)))
-#define FLOAT_QUAT_NORMALIZE(_q) { \
+#define FLOAT_QUAT_NORMALIZE(_q) { \
float norm = FLOAT_QUAT_NORM(_q); \
if (norm > FLT_MIN) { \
(_q).qi = (_q).qi / norm; \
@@ -515,20 +488,20 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) {
#define FLOAT_QUAT_INVERT(_qo, _qi) QUAT_INVERT(_qo, _qi)
-#define FLOAT_QUAT_WRAP_SHORTEST(_q) { \
+#define FLOAT_QUAT_WRAP_SHORTEST(_q) { \
if ((_q).qi < 0.) \
- QUAT_EXPLEMENTARY(_q,_q); \
+ QUAT_EXPLEMENTARY(_q,_q); \
}
/* _a2c = _a2b comp _b2c , aka _a2c = _a2b * _b2c */
-#define FLOAT_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c) { \
- FLOAT_QUAT_COMP(_a2c, _a2b, _b2c); \
- FLOAT_QUAT_WRAP_SHORTEST(_a2c); \
- FLOAT_QUAT_NORMALIZE(_a2c); \
+#define FLOAT_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c) { \
+ FLOAT_QUAT_COMP(_a2c, _a2b, _b2c); \
+ FLOAT_QUAT_WRAP_SHORTEST(_a2c); \
+ FLOAT_QUAT_NORMALIZE(_a2c); \
}
/* _a2c = _a2b comp _b2c , aka _a2c = _a2b * _b2c */
-#define FLOAT_QUAT_COMP(_a2c, _a2b, _b2c) { \
+#define FLOAT_QUAT_COMP(_a2c, _a2b, _b2c) { \
(_a2c).qi = (_a2b).qi*(_b2c).qi - (_a2b).qx*(_b2c).qx - (_a2b).qy*(_b2c).qy - (_a2b).qz*(_b2c).qz; \
(_a2c).qx = (_a2b).qi*(_b2c).qx + (_a2b).qx*(_b2c).qi + (_a2b).qy*(_b2c).qz - (_a2b).qz*(_b2c).qy; \
(_a2c).qy = (_a2b).qi*(_b2c).qy - (_a2b).qx*(_b2c).qz + (_a2b).qy*(_b2c).qi + (_a2b).qz*(_b2c).qx; \
@@ -538,14 +511,14 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) {
#define FLOAT_QUAT_MULT(_a2c, _a2b, _b2c) FLOAT_QUAT_COMP(_a2c, _a2b, _b2c)
/* _a2b = _a2c comp_inv _b2c , aka _a2b = _a2c * inv(_b2c) */
-#define FLOAT_QUAT_COMP_INV_NORM_SHORTEST(_a2b, _a2c, _b2c) { \
- FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c); \
- FLOAT_QUAT_WRAP_SHORTEST(_a2b); \
- FLOAT_QUAT_NORMALIZE(_a2b); \
+#define FLOAT_QUAT_COMP_INV_NORM_SHORTEST(_a2b, _a2c, _b2c) { \
+ FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c); \
+ FLOAT_QUAT_WRAP_SHORTEST(_a2b); \
+ FLOAT_QUAT_NORMALIZE(_a2b); \
}
/* _a2b = _a2c comp_inv _b2c , aka _a2b = _a2c * inv(_b2c) */
-#define FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c) { \
+#define FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c) { \
(_a2b).qi = (_a2c).qi*(_b2c).qi + (_a2c).qx*(_b2c).qx + (_a2c).qy*(_b2c).qy + (_a2c).qz*(_b2c).qz; \
(_a2b).qx = -(_a2c).qi*(_b2c).qx + (_a2c).qx*(_b2c).qi - (_a2c).qy*(_b2c).qz + (_a2c).qz*(_b2c).qy; \
(_a2b).qy = -(_a2c).qi*(_b2c).qy + (_a2c).qx*(_b2c).qz + (_a2c).qy*(_b2c).qi - (_a2c).qz*(_b2c).qx; \
@@ -553,110 +526,83 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) {
}
/* _b2c = _a2b inv_comp _a2c , aka _b2c = inv(_a2b) * _a2c */
-#define FLOAT_QUAT_INV_COMP_NORM_SHORTEST(_b2c, _a2b, _a2c) { \
- FLOAT_QUAT_INV_COMP(_b2c, _a2b, _a2c); \
- FLOAT_QUAT_WRAP_SHORTEST(_b2c); \
- FLOAT_QUAT_NORMALIZE(_b2c); \
+#define FLOAT_QUAT_INV_COMP_NORM_SHORTEST(_b2c, _a2b, _a2c) { \
+ FLOAT_QUAT_INV_COMP(_b2c, _a2b, _a2c); \
+ FLOAT_QUAT_WRAP_SHORTEST(_b2c); \
+ FLOAT_QUAT_NORMALIZE(_b2c); \
}
/* _b2c = _a2b inv_comp _a2c , aka _b2c = inv(_a2b) * _a2c */
-#define FLOAT_QUAT_INV_COMP(_b2c, _a2b, _a2c) { \
+#define FLOAT_QUAT_INV_COMP(_b2c, _a2b, _a2c) { \
(_b2c).qi = (_a2b).qi*(_a2c).qi + (_a2b).qx*(_a2c).qx + (_a2b).qy*(_a2c).qy + (_a2b).qz*(_a2c).qz; \
(_b2c).qx = (_a2b).qi*(_a2c).qx - (_a2b).qx*(_a2c).qi - (_a2b).qy*(_a2c).qz + (_a2b).qz*(_a2c).qy; \
(_b2c).qy = (_a2b).qi*(_a2c).qy + (_a2b).qx*(_a2c).qz - (_a2b).qy*(_a2c).qi - (_a2b).qz*(_a2c).qx; \
(_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); \
- if (v_norm < 1e-8) { \
- (q_out).qi = 1; \
- (q_out).qx = 0; \
- (q_out).qy = 0; \
- (q_out).qz = 0; \
- } else { \
- (q_out).qi = c2; \
- (q_out).qx = (w).p/v_norm * s2; \
- (q_out).qy = (w).q/v_norm * s2; \
- (q_out).qz = (w).r/v_norm * s2; \
- } \
+ const float c2 = cos(dt*v_norm/2.0); \
+ const float s2 = sin(dt*v_norm/2.0); \
+ if (v_norm < 1e-8) { \
+ (q_out).qi = 1; \
+ (q_out).qx = 0; \
+ (q_out).qy = 0; \
+ (q_out).qz = 0; \
+ } else { \
+ (q_out).qi = c2; \
+ (q_out).qx = (w).p/v_norm * s2; \
+ (q_out).qy = (w).q/v_norm * s2; \
+ (q_out).qz = (w).r/v_norm * s2; \
+ } \
}
/* 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; \
- } \
+#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; \
- const float qiqx = q.qi*q.qx; \
- const float qiqy = q.qi*q.qy; \
- const float qiqz = q.qi*q.qz; \
- const float qx2 = q.qx*q.qx; \
- const float qxqy = q.qx*q.qy; \
- const float qxqz = q.qx*q.qz; \
- const float qy2 = q.qy*q.qy; \
- const float qyqz = q.qy*q.qz; \
- const float qz2 = q.qz*q.qz; \
- const float m00 = qi2 + qx2 - qy2 - qz2; \
- const float m01 = 2 * ( qxqy + qiqz ); \
- const float m02 = 2 * ( qxqz - qiqy ); \
- const float m10 = 2 * ( qxqy - qiqz ); \
- const float m11 = qi2 - qx2 + qy2 - qz2; \
- const float m12 = 2 * ( qyqz + qiqx ); \
- const float m20 = 2 * ( qxqz + qiqy ); \
- const float m21 = 2 * ( qyqz - qiqx ); \
- const float m22 = qi2 - qx2 - qy2 + qz2; \
- v_out.x = m00 * v_in.x + m01 * v_in.y + m02 * v_in.z; \
- v_out.y = m10 * v_in.x + m11 * v_in.y + m12 * v_in.z; \
- v_out.z = m20 * v_in.x + m21 * v_in.y + m22 * v_in.z; \
- }
-#else
-#define FLOAT_QUAT_VMULT(v_out, q, v_in) { \
- const float qi2_M1_2 = q.qi*q.qi - 0.5; \
- const float qiqx = q.qi*q.qx; \
- const float qiqy = q.qi*q.qy; \
- const float qiqz = q.qi*q.qz; \
- float m01 = q.qx*q.qy; /* aka qxqy */ \
- float m02 = q.qx*q.qz; /* aka qxqz */ \
- float m12 = q.qy*q.qz; /* aka qyqz */ \
+#define FLOAT_QUAT_VMULT(v_out, q, v_in) { \
+ const float qi2_M1_2 = (q).qi*(q).qi - 0.5; \
+ const float qiqx = (q).qi*(q).qx; \
+ const float qiqy = (q).qi*(q).qy; \
+ const float qiqz = (q).qi*(q).qz; \
+ float m01 = (q).qx*(q).qy; /* aka qxqy */ \
+ float m02 = (q).qx*(q).qz; /* aka qxqz */ \
+ float m12 = (q).qy*(q).qz; /* aka qyqz */ \
\
- const float m00 = qi2_M1_2 + q.qx*q.qx; \
- const float m10 = m01 - qiqz; \
- const float m20 = m02 + qiqy; \
- const float m21 = m12 - qiqx; \
- m01 += qiqz; \
- m02 -= qiqy; \
- m12 += qiqx; \
- const float m11 = qi2_M1_2 + q.qy*q.qy; \
- const float m22 = qi2_M1_2 + q.qz*q.qz; \
- v_out.x = 2*(m00 * v_in.x + m01 * v_in.y + m02 * v_in.z); \
- v_out.y = 2*(m10 * v_in.x + m11 * v_in.y + m12 * v_in.z); \
- v_out.z = 2*(m20 * v_in.x + m21 * v_in.y + m22 * v_in.z); \
+ const float m00 = qi2_M1_2 + (q).qx*(q).qx; \
+ const float m10 = m01 - qiqz; \
+ const float m20 = m02 + qiqy; \
+ const float m21 = m12 - qiqx; \
+ m01 += qiqz; \
+ m02 -= qiqy; \
+ m12 += qiqx; \
+ const float m11 = qi2_M1_2 + (q).qy*(q).qy; \
+ const float m22 = qi2_M1_2 + (q).qz*(q).qz; \
+ (v_out).x = 2*(m00 * (v_in).x + m01 * (v_in).y + m02 * (v_in).z); \
+ (v_out).y = 2*(m10 * (v_in).x + m11 * (v_in).y + m12 * (v_in).z); \
+ (v_out).z = 2*(m20 * (v_in).x + m21 * (v_in).y + m22 * (v_in).z); \
}
-#endif
/* _qd = -0.5*omega(_r) * _q */
-#define FLOAT_QUAT_DERIVATIVE(_qd, _r, _q) { \
+#define FLOAT_QUAT_DERIVATIVE(_qd, _r, _q) { \
(_qd).qi = -0.5*( (_r).p*(_q).qx + (_r).q*(_q).qy + (_r).r*(_q).qz); \
(_qd).qx = -0.5*(-(_r).p*(_q).qi - (_r).r*(_q).qy + (_r).q*(_q).qz); \
(_qd).qy = -0.5*(-(_r).q*(_q).qi + (_r).r*(_q).qx - (_r).p*(_q).qz); \
@@ -664,87 +610,103 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) {
}
/* _qd = -0.5*omega(_r) * _q */
-#define FLOAT_QUAT_DERIVATIVE_LAGRANGE(_qd, _r, _q) { \
- const float K_LAGRANGE = 1.; \
- const float c = K_LAGRANGE * ( 1 - FLOAT_QUAT_NORM(_q)) / -0.5; \
+#define FLOAT_QUAT_DERIVATIVE_LAGRANGE(_qd, _r, _q) { \
+ const float K_LAGRANGE = 1.; \
+ const float c = K_LAGRANGE * ( 1 - FLOAT_QUAT_NORM(_q)) / -0.5; \
(_qd).qi = -0.5*( c*(_q).qi + (_r).p*(_q).qx + (_r).q*(_q).qy + (_r).r*(_q).qz); \
(_qd).qx = -0.5*(-(_r).p*(_q).qi + c*(_q).qx - (_r).r*(_q).qy + (_r).q*(_q).qz); \
(_qd).qy = -0.5*(-(_r).q*(_q).qi + (_r).r*(_q).qx + c*(_q).qy - (_r).p*(_q).qz); \
(_qd).qz = -0.5*(-(_r).r*(_q).qi - (_r).q*(_q).qx + (_r).p*(_q).qy + c*(_q).qz); \
}
-#define FLOAT_QUAT_OF_EULERS(_q, _e) { \
- \
- const float phi2 = (_e).phi/2.0; \
- const float theta2 = (_e).theta/2.0; \
- const float psi2 = (_e).psi/2.0; \
- \
- const float s_phi2 = sinf( phi2 ); \
- const float c_phi2 = cosf( phi2 ); \
- const float s_theta2 = sinf( theta2 ); \
- const float c_theta2 = cosf( theta2 ); \
- const float s_psi2 = sinf( psi2 ); \
- const float c_psi2 = cosf( psi2 ); \
- \
+#define FLOAT_QUAT_OF_EULERS(_q, _e) { \
+ \
+ const float phi2 = (_e).phi/2.0; \
+ const float theta2 = (_e).theta/2.0; \
+ const float psi2 = (_e).psi/2.0; \
+ \
+ const float s_phi2 = sinf( phi2 ); \
+ const float c_phi2 = cosf( phi2 ); \
+ const float s_theta2 = sinf( theta2 ); \
+ const float c_theta2 = cosf( theta2 ); \
+ const float s_psi2 = sinf( psi2 ); \
+ const float c_psi2 = cosf( psi2 ); \
+ \
(_q).qi = c_phi2 * c_theta2 * c_psi2 + s_phi2 * s_theta2 * s_psi2; \
(_q).qx = -c_phi2 * s_theta2 * s_psi2 + s_phi2 * c_theta2 * c_psi2; \
(_q).qy = c_phi2 * s_theta2 * c_psi2 + s_phi2 * c_theta2 * s_psi2; \
(_q).qz = c_phi2 * c_theta2 * s_psi2 - s_phi2 * s_theta2 * c_psi2; \
- \
+ \
}
-#define FLOAT_QUAT_OF_AXIS_ANGLE(_q, _uv, _an) { \
- const float san = sinf(_an/2.); \
- _q.qi = cosf(_an/2.); \
- _q.qx = san * _uv.x; \
- _q.qy = san * _uv.y; \
- _q.qz = san * _uv.z; \
+#define FLOAT_QUAT_OF_AXIS_ANGLE(_q, _uv, _an) { \
+ const float san = sinf((_an)/2.); \
+ (_q).qi = cosf((_an)/2.); \
+ (_q).qx = san * (_uv).x; \
+ (_q).qy = san * (_uv).y; \
+ (_q).qz = san * (_uv).z; \
}
-#define FLOAT_QUAT_OF_RMAT(_q, _r) { \
- const float tr = RMAT_TRACE(_r); \
- if (tr > 0) { \
- const float two_qi = sqrtf(1.+tr); \
- const float four_qi = 2. * two_qi; \
- _q.qi = 0.5 * two_qi; \
- _q.qx = (RMAT_ELMT(_r, 1, 2)-RMAT_ELMT(_r, 2, 1))/four_qi; \
- _q.qy = (RMAT_ELMT(_r, 2, 0)-RMAT_ELMT(_r, 0, 2))/four_qi; \
- _q.qz = (RMAT_ELMT(_r, 0, 1)-RMAT_ELMT(_r, 1, 0))/four_qi; \
- /*printf("tr > 0\n");*/ \
- } \
- else { \
- if (RMAT_ELMT(_r, 0, 0) > RMAT_ELMT(_r, 1, 1) && \
- RMAT_ELMT(_r, 0, 0) > RMAT_ELMT(_r, 2, 2)) { \
- const float two_qx = sqrtf(RMAT_ELMT(_r, 0, 0) -RMAT_ELMT(_r, 1, 1) \
- -RMAT_ELMT(_r, 2, 2) + 1); \
- const float four_qx = 2. * two_qx; \
- _q.qi = (RMAT_ELMT(_r, 1, 2)-RMAT_ELMT(_r, 2, 1))/four_qx; \
- _q.qx = 0.5 * two_qx; \
- _q.qy = (RMAT_ELMT(_r, 0, 1)+RMAT_ELMT(_r, 1, 0))/four_qx; \
- _q.qz = (RMAT_ELMT(_r, 2, 0)+RMAT_ELMT(_r, 0, 2))/four_qx; \
- /*printf("m00 largest\n");*/ \
- } \
- else if (RMAT_ELMT(_r, 1, 1) > RMAT_ELMT(_r, 2, 2)) { \
- const float two_qy = \
- sqrtf(RMAT_ELMT(_r, 1, 1) - RMAT_ELMT(_r, 0, 0) - RMAT_ELMT(_r, 2, 2) + 1); \
- const float four_qy = 2. * two_qy; \
- _q.qi = (RMAT_ELMT(_r, 2, 0) - RMAT_ELMT(_r, 0, 2))/four_qy; \
- _q.qx = (RMAT_ELMT(_r, 0, 1) + RMAT_ELMT(_r, 1, 0))/four_qy; \
- _q.qy = 0.5 * two_qy; \
- _q.qz = (RMAT_ELMT(_r, 1, 2) + RMAT_ELMT(_r, 2, 1))/four_qy; \
- /*printf("m11 largest\n");*/ \
- } \
- else { \
- const float two_qz = \
- sqrtf(RMAT_ELMT(_r, 2, 2) - RMAT_ELMT(_r, 0, 0) - RMAT_ELMT(_r, 1, 1) + 1); \
- const float four_qz = 2. * two_qz; \
- _q.qi = (RMAT_ELMT(_r, 0, 1)- RMAT_ELMT(_r, 1, 0))/four_qz; \
- _q.qx = (RMAT_ELMT(_r, 2, 0)+ RMAT_ELMT(_r, 0, 2))/four_qz; \
- _q.qy = (RMAT_ELMT(_r, 1, 2)+ RMAT_ELMT(_r, 2, 1))/four_qz; \
- _q.qz = 0.5 * two_qz; \
- /*printf("m22 largest\n");*/ \
- } \
- } \
+#define FLOAT_QUAT_OF_ORIENTATION_VECT(_q, _ov) { \
+ const float ov_norm = sqrtf((_ov).x*(_ov).x + (_ov).y*(_ov).y + (_ov).z*(_ov).z); \
+ if (ov_norm < 1e-8) { \
+ (_q).qi = 1; \
+ (_q).qx = 0; \
+ (_q).qy = 0; \
+ (_q).qz = 0; \
+ } else { \
+ const float s2_normalized = sinf(ov_norm/2.0) / ov_norm; \
+ (_q).qi = cosf(ov_norm/2.0); \
+ (_q).qx = (_ov).x * s2_normalized; \
+ (_q).qy = (_ov).y * s2_normalized; \
+ (_q).qz = (_ov).z * s2_normalized; \
+ } \
+ }
+
+#define FLOAT_QUAT_OF_RMAT(_q, _r) { \
+ const float tr = RMAT_TRACE((_r)); \
+ if (tr > 0) { \
+ const float two_qi = sqrtf(1.+tr); \
+ const float four_qi = 2. * two_qi; \
+ (_q).qi = 0.5 * two_qi; \
+ (_q).qx = (RMAT_ELMT((_r), 1, 2)-RMAT_ELMT((_r), 2, 1))/four_qi; \
+ (_q).qy = (RMAT_ELMT((_r), 2, 0)-RMAT_ELMT((_r), 0, 2))/four_qi; \
+ (_q).qz = (RMAT_ELMT((_r), 0, 1)-RMAT_ELMT((_r), 1, 0))/four_qi; \
+ /*printf("tr > 0\n");*/ \
+ } \
+ else { \
+ if (RMAT_ELMT((_r), 0, 0) > RMAT_ELMT((_r), 1, 1) && \
+ RMAT_ELMT((_r), 0, 0) > RMAT_ELMT((_r), 2, 2)) { \
+ const float two_qx = sqrtf(RMAT_ELMT((_r), 0, 0) -RMAT_ELMT((_r), 1, 1) \
+ -RMAT_ELMT((_r), 2, 2) + 1); \
+ const float four_qx = 2. * two_qx; \
+ (_q).qi = (RMAT_ELMT((_r), 1, 2)-RMAT_ELMT((_r), 2, 1))/four_qx; \
+ (_q).qx = 0.5 * two_qx; \
+ (_q).qy = (RMAT_ELMT((_r), 0, 1)+RMAT_ELMT((_r), 1, 0))/four_qx; \
+ (_q).qz = (RMAT_ELMT((_r), 2, 0)+RMAT_ELMT((_r), 0, 2))/four_qx; \
+ /*printf("m00 largest\n");*/ \
+ } \
+ else if (RMAT_ELMT((_r), 1, 1) > RMAT_ELMT((_r), 2, 2)) { \
+ const float two_qy = \
+ sqrtf(RMAT_ELMT((_r), 1, 1) - RMAT_ELMT((_r), 0, 0) - RMAT_ELMT((_r), 2, 2) + 1); \
+ const float four_qy = 2. * two_qy; \
+ (_q).qi = (RMAT_ELMT((_r), 2, 0) - RMAT_ELMT((_r), 0, 2))/four_qy; \
+ (_q).qx = (RMAT_ELMT((_r), 0, 1) + RMAT_ELMT((_r), 1, 0))/four_qy; \
+ (_q).qy = 0.5 * two_qy; \
+ (_q).qz = (RMAT_ELMT((_r), 1, 2) + RMAT_ELMT((_r), 2, 1))/four_qy; \
+ /*printf("m11 largest\n");*/ \
+ } \
+ else { \
+ const float two_qz = \
+ sqrtf(RMAT_ELMT((_r), 2, 2) - RMAT_ELMT((_r), 0, 0) - RMAT_ELMT((_r), 1, 1) + 1); \
+ const float four_qz = 2. * two_qz; \
+ (_q).qi = (RMAT_ELMT((_r), 0, 1)- RMAT_ELMT((_r), 1, 0))/four_qz; \
+ (_q).qx = (RMAT_ELMT((_r), 2, 0)+ RMAT_ELMT((_r), 0, 2))/four_qz; \
+ (_q).qy = (RMAT_ELMT((_r), 1, 2)+ RMAT_ELMT((_r), 2, 1))/four_qz; \
+ (_q).qz = 0.5 * two_qz; \
+ /*printf("m22 largest\n");*/ \
+ } \
+ } \
}
@@ -756,40 +718,40 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) {
#define FLOAT_EULERS_NORM(_e) (sqrtf(SQUARE((_e).phi)+SQUARE((_e).theta)+SQUARE((_e).psi))) ;
-#define FLOAT_EULERS_OF_RMAT(_e, _rm) { \
- \
- const float dcm00 = (_rm).m[0]; \
- const float dcm01 = (_rm).m[1]; \
- const float dcm02 = (_rm).m[2]; \
- const float dcm12 = (_rm).m[5]; \
- const float dcm22 = (_rm).m[8]; \
- (_e).phi = atan2f( dcm12, dcm22 ); \
- (_e).theta = -asinf( dcm02 ); \
- (_e).psi = atan2f( dcm01, dcm00 ); \
- \
+#define FLOAT_EULERS_OF_RMAT(_e, _rm) { \
+ \
+ const float dcm00 = (_rm).m[0]; \
+ const float dcm01 = (_rm).m[1]; \
+ const float dcm02 = (_rm).m[2]; \
+ const float dcm12 = (_rm).m[5]; \
+ const float dcm22 = (_rm).m[8]; \
+ (_e).phi = atan2f( dcm12, dcm22 ); \
+ (_e).theta = -asinf( dcm02 ); \
+ (_e).psi = atan2f( dcm01, dcm00 ); \
+ \
}
-#define FLOAT_EULERS_OF_QUAT(_e, _q) { \
- \
- const float qx2 = (_q).qx*(_q).qx; \
- const float qy2 = (_q).qy*(_q).qy; \
- const float qz2 = (_q).qz*(_q).qz; \
- const float qiqx = (_q).qi*(_q).qx; \
- const float qiqy = (_q).qi*(_q).qy; \
- const float qiqz = (_q).qi*(_q).qz; \
- const float qxqy = (_q).qx*(_q).qy; \
- const float qxqz = (_q).qx*(_q).qz; \
- const float qyqz = (_q).qy*(_q).qz; \
- const float dcm00 = 1.0 - 2.*( qy2 + qz2 ); \
- const float dcm01 = 2.*( qxqy + qiqz ); \
- const float dcm02 = 2.*( qxqz - qiqy ); \
- const float dcm12 = 2.*( qyqz + qiqx ); \
- const float dcm22 = 1.0 - 2.*( qx2 + qy2 ); \
- \
- (_e).phi = atan2f( dcm12, dcm22 ); \
- (_e).theta = -asinf( dcm02 ); \
- (_e).psi = atan2f( dcm01, dcm00 ); \
- \
+#define FLOAT_EULERS_OF_QUAT(_e, _q) { \
+ \
+ const float qx2 = (_q).qx*(_q).qx; \
+ const float qy2 = (_q).qy*(_q).qy; \
+ const float qz2 = (_q).qz*(_q).qz; \
+ const float qiqx = (_q).qi*(_q).qx; \
+ const float qiqy = (_q).qi*(_q).qy; \
+ const float qiqz = (_q).qi*(_q).qz; \
+ const float qxqy = (_q).qx*(_q).qy; \
+ const float qxqz = (_q).qx*(_q).qz; \
+ const float qyqz = (_q).qy*(_q).qz; \
+ const float dcm00 = 1.0 - 2.*( qy2 + qz2 ); \
+ const float dcm01 = 2.*( qxqy + qiqz ); \
+ const float dcm02 = 2.*( qxqz - qiqy ); \
+ const float dcm12 = 2.*( qyqz + qiqx ); \
+ const float dcm22 = 1.0 - 2.*( qx2 + qy2 ); \
+ \
+ (_e).phi = atan2f( dcm12, dcm22 ); \
+ (_e).theta = -asinf( dcm02 ); \
+ (_e).psi = atan2f( dcm01, dcm00 ); \
+ \
}
diff --git a/sw/airborne/math/pprz_algebra_int.h b/sw/airborne/math/pprz_algebra_int.h
index 3df0026633..50f7a1e88c 100644
--- a/sw/airborne/math/pprz_algebra_int.h
+++ b/sw/airborne/math/pprz_algebra_int.h
@@ -106,14 +106,14 @@ struct Int64Quat {
#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; \
+#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; \
}
-#define INT32_COURSE_NORMALIZE(_a) { \
+#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) >= INT32_ANGLE_2_PI) (_a) -= INT32_ANGLE_2_PI; \
}
@@ -213,9 +213,9 @@ struct Int64Vect3 {
#define INT_VECT2_ASSIGN(_a, _x, _y) VECT2_ASSIGN(_a, _x, _y)
-#define INT32_VECT2_NORM(n, v) { \
+#define INT32_VECT2_NORM(n, v) { \
int32_t n2 = (v).x*(v).x + (v).y*(v).y; \
- INT32_SQRT(n, n2); \
+ INT32_SQRT(n, n2); \
}
#define INT32_VECT2_RSHIFT(_o, _i, _r) { \
@@ -251,36 +251,36 @@ struct Int64Vect3 {
#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); \
- (_a).y = ((_b).y * (_num)) / (_den); \
- (_a).z = ((_b).z * (_num)) / (_den); \
+#define INT32_VECT3_SCALE_2(_a, _b, _num, _den) { \
+ (_a).x = ((_b).x * (_num)) / (_den); \
+ (_a).y = ((_b).y * (_num)) / (_den); \
+ (_a).z = ((_b).z * (_num)) / (_den); \
}
#define INT32_VECT3_SDIV(_a, _b, _s) VECT3_SDIV(_a, _b, _s)
-#define INT32_VECT3_NORM(n, v) { \
+#define INT32_VECT3_NORM(n, v) { \
int32_t n2 = (v).x*(v).x + (v).y*(v).y + (v).z*(v).z; \
- INT32_SQRT(n, n2); \
+ INT32_SQRT(n, n2); \
}
#define INT32_VECT3_RSHIFT(_o, _i, _r) { \
- (_o).x = ((_i).x >> (_r)); \
- (_o).y = ((_i).y >> (_r)); \
- (_o).z = ((_i).z >> (_r)); \
+ (_o).x = ((_i).x >> (_r)); \
+ (_o).y = ((_i).y >> (_r)); \
+ (_o).z = ((_i).z >> (_r)); \
}
#define INT32_VECT3_LSHIFT(_o, _i, _l) { \
- (_o).x = ((_i).x << (_l)); \
- (_o).y = ((_i).y << (_l)); \
- (_o).z = ((_i).z << (_l)); \
+ (_o).x = ((_i).x << (_l)); \
+ (_o).y = ((_i).y << (_l)); \
+ (_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; \
+#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; \
}
@@ -288,46 +288,46 @@ 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; \
+#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; \
}
-#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); \
+#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); \
}
-#define INT32_MAT33_VECT3_MUL(_o, _m, _v, _f) { \
- (_o).x = ((_m).m[0]*(_v).x + (_m).m[1]*(_v).y + (_m).m[2]*(_v).z)>>(_f); \
- (_o).y = ((_m).m[3]*(_v).x + (_m).m[4]*(_v).y + (_m).m[5]*(_v).z)>>(_f); \
- (_o).z = ((_m).m[6]*(_v).x + (_m).m[7]*(_v).y + (_m).m[8]*(_v).z)>>(_f); \
+#define INT32_MAT33_VECT3_MUL(_o, _m, _v, _f) { \
+ (_o).x = ((_m).m[0]*(_v).x + (_m).m[1]*(_v).y + (_m).m[2]*(_v).z)>>(_f); \
+ (_o).y = ((_m).m[3]*(_v).x + (_m).m[4]*(_v).y + (_m).m[5]*(_v).z)>>(_f); \
+ (_o).z = ((_m).m[6]*(_v).x + (_m).m[7]*(_v).y + (_m).m[8]*(_v).z)>>(_f); \
}
/*
* Rotation matrices
*/
-#define INT32_RMAT_ZERO(_rm) \
+#define INT32_RMAT_ZERO(_rm) \
INT32_MAT33_DIAG(_rm, TRIG_BFP_OF_REAL( 1.), TRIG_BFP_OF_REAL( 1.), TRIG_BFP_OF_REAL( 1.))
/* _m_a2c = _m_a2b comp _m_b2c , aka _m_a2c = _m_b2c * _m_a2b */
-#define INT32_RMAT_COMP(_m_a2c, _m_a2b, _m_b2c) { \
+#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; \
@@ -340,7 +340,7 @@ struct Int64Vect3 {
}
/* _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) { \
+#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; \
@@ -353,13 +353,13 @@ struct Int64Vect3 {
}
/* _vb = _m_a2b * _va */
-#define INT32_RMAT_VMULT(_vb, _m_a2b, _va) { \
+#define INT32_RMAT_VMULT(_vb, _m_a2b, _va) { \
(_vb).x = ( (_m_a2b).m[0]*(_va).x + (_m_a2b).m[1]*(_va).y + (_m_a2b).m[2]*(_va).z)>>INT32_TRIG_FRAC; \
(_vb).y = ( (_m_a2b).m[3]*(_va).x + (_m_a2b).m[4]*(_va).y + (_m_a2b).m[5]*(_va).z)>>INT32_TRIG_FRAC; \
(_vb).z = ( (_m_a2b).m[6]*(_va).x + (_m_a2b).m[7]*(_va).y + (_m_a2b).m[8]*(_va).z)>>INT32_TRIG_FRAC; \
}
-#define INT32_RMAT_TRANSP_VMULT(_vb, _m_b2a, _va) { \
+#define INT32_RMAT_TRANSP_VMULT(_vb, _m_b2a, _va) { \
(_vb).x = ( (_m_b2a).m[0]*(_va).x + (_m_b2a).m[3]*(_va).y + (_m_b2a).m[6]*(_va).z)>>INT32_TRIG_FRAC; \
(_vb).y = ( (_m_b2a).m[1]*(_va).x + (_m_b2a).m[4]*(_va).y + (_m_b2a).m[7]*(_va).z)>>INT32_TRIG_FRAC; \
(_vb).z = ( (_m_b2a).m[2]*(_va).x + (_m_b2a).m[5]*(_va).y + (_m_b2a).m[8]*(_va).z)>>INT32_TRIG_FRAC; \
@@ -371,7 +371,7 @@ struct Int64Vect3 {
(_vb).r = ( (_m_a2b).m[6]*(_va).p + (_m_a2b).m[7]*(_va).q + (_m_a2b).m[8]*(_va).r)>>INT32_TRIG_FRAC; \
}
-#define INT32_RMAT_TRANSP_RATEMULT(_vb, _m_b2a, _va) { \
+#define INT32_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)>>INT32_TRIG_FRAC; \
(_vb).q = ( (_m_b2a).m[1]*(_va).p + (_m_b2a).m[4]*(_va).q + (_m_b2a).m[7]*(_va).r)>>INT32_TRIG_FRAC; \
(_vb).r = ( (_m_b2a).m[2]*(_va).p + (_m_b2a).m[5]*(_va).q + (_m_b2a).m[8]*(_va).r)>>INT32_TRIG_FRAC; \
@@ -381,63 +381,29 @@ struct Int64Vect3 {
/*
* http://www.mathworks.com/access/helpdesk_r13/help/toolbox/aeroblks/quaternionstodirectioncosinematrix.html
*/
-#ifdef ALGEBRA_INT_USE_SLOW_FUNCTIONS
-#define INT32_RMAT_OF_QUAT(_rm, _q) { \
- const int32_t qx2 = INT_MULT_RSHIFT((_q).qx,(_q).qx, INT32_QUAT_FRAC); \
- const int32_t qy2 = INT_MULT_RSHIFT((_q).qy,(_q).qy, INT32_QUAT_FRAC); \
- const int32_t qz2 = INT_MULT_RSHIFT((_q).qz,(_q).qz, INT32_QUAT_FRAC); \
- const int32_t qiqx = INT_MULT_RSHIFT((_q).qi,(_q).qx, INT32_QUAT_FRAC); \
- const int32_t qiqy = INT_MULT_RSHIFT((_q).qi,(_q).qy, INT32_QUAT_FRAC); \
- const int32_t qiqz = INT_MULT_RSHIFT((_q).qi,(_q).qz, INT32_QUAT_FRAC); \
- const int32_t qxqy = INT_MULT_RSHIFT((_q).qx,(_q).qy, INT32_QUAT_FRAC); \
- const int32_t qxqz = INT_MULT_RSHIFT((_q).qx,(_q).qz, INT32_QUAT_FRAC); \
- const int32_t qyqz = INT_MULT_RSHIFT((_q).qy,(_q).qz, INT32_QUAT_FRAC); \
- 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); \
- /* dcm01 = 2.*( qxqy + qiqz ); */ \
- (_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); \
- /* dcm10 = 2.*( qxqy - qiqz ); */ \
- (_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); \
- /* dcm12 = 2.*( qyqz + qiqx ); */ \
- (_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); \
- /* dcm21 = 2.*( qyqz - qiqx ); */ \
- (_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); \
+#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); \
+ \
+ 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[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; \
}
-#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); \
- \
- 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[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
/*
@@ -446,21 +412,21 @@ struct Int64Vect3 {
#define INT32_RMAT_OF_EULERS(_rm, _e) INT32_RMAT_OF_EULERS_321(_rm, _e)
-#define INT32_RMAT_OF_EULERS_321(_rm, _e) { \
- \
- int32_t sphi; \
- PPRZ_ITRIG_SIN(sphi, (_e).phi); \
- int32_t cphi; \
- PPRZ_ITRIG_COS(cphi, (_e).phi); \
- int32_t stheta; \
- PPRZ_ITRIG_SIN(stheta, (_e).theta); \
- int32_t ctheta; \
- PPRZ_ITRIG_COS(ctheta, (_e).theta); \
- int32_t spsi; \
- PPRZ_ITRIG_SIN(spsi, (_e).psi); \
- int32_t cpsi; \
- PPRZ_ITRIG_COS(cpsi, (_e).psi); \
- \
+#define INT32_RMAT_OF_EULERS_321(_rm, _e) { \
+ \
+ int32_t sphi; \
+ PPRZ_ITRIG_SIN(sphi, (_e).phi); \
+ int32_t cphi; \
+ PPRZ_ITRIG_COS(cphi, (_e).phi); \
+ int32_t stheta; \
+ PPRZ_ITRIG_SIN(stheta, (_e).theta); \
+ int32_t ctheta; \
+ PPRZ_ITRIG_COS(ctheta, (_e).theta); \
+ int32_t spsi; \
+ PPRZ_ITRIG_SIN(spsi, (_e).psi); \
+ int32_t cpsi; \
+ PPRZ_ITRIG_COS(cpsi, (_e).psi); \
+ \
int32_t ctheta_cpsi = INT_MULT_RSHIFT(ctheta, cpsi, INT32_TRIG_FRAC); \
int32_t ctheta_spsi = INT_MULT_RSHIFT(ctheta, spsi, INT32_TRIG_FRAC); \
int32_t cphi_spsi = INT_MULT_RSHIFT(cphi, spsi, INT32_TRIG_FRAC); \
@@ -471,41 +437,41 @@ struct Int64Vect3 {
int32_t sphi_stheta = INT_MULT_RSHIFT(sphi, stheta, INT32_TRIG_FRAC); \
int32_t sphi_spsi = INT_MULT_RSHIFT(sphi, spsi, INT32_TRIG_FRAC); \
int32_t sphi_cpsi = INT_MULT_RSHIFT(sphi, cpsi, INT32_TRIG_FRAC); \
- \
+ \
int32_t sphi_stheta_cpsi = INT_MULT_RSHIFT(sphi_stheta, cpsi, INT32_TRIG_FRAC); \
int32_t sphi_stheta_spsi = INT_MULT_RSHIFT(sphi_stheta, spsi, INT32_TRIG_FRAC); \
int32_t cphi_stheta_cpsi = INT_MULT_RSHIFT(cphi_stheta, cpsi, INT32_TRIG_FRAC); \
int32_t cphi_stheta_spsi = INT_MULT_RSHIFT(cphi_stheta, spsi, INT32_TRIG_FRAC); \
- \
- RMAT_ELMT(_rm, 0, 0) = ctheta_cpsi; \
- RMAT_ELMT(_rm, 0, 1) = ctheta_spsi; \
- RMAT_ELMT(_rm, 0, 2) = -stheta; \
- RMAT_ELMT(_rm, 1, 0) = sphi_stheta_cpsi - cphi_spsi; \
- RMAT_ELMT(_rm, 1, 1) = sphi_stheta_spsi + cphi_cpsi; \
- RMAT_ELMT(_rm, 1, 2) = sphi_ctheta; \
- RMAT_ELMT(_rm, 2, 0) = cphi_stheta_cpsi + sphi_spsi; \
- RMAT_ELMT(_rm, 2, 1) = cphi_stheta_spsi - sphi_cpsi; \
- RMAT_ELMT(_rm, 2, 2) = cphi_ctheta; \
- \
+ \
+ RMAT_ELMT(_rm, 0, 0) = ctheta_cpsi; \
+ RMAT_ELMT(_rm, 0, 1) = ctheta_spsi; \
+ RMAT_ELMT(_rm, 0, 2) = -stheta; \
+ RMAT_ELMT(_rm, 1, 0) = sphi_stheta_cpsi - cphi_spsi; \
+ RMAT_ELMT(_rm, 1, 1) = sphi_stheta_spsi + cphi_cpsi; \
+ RMAT_ELMT(_rm, 1, 2) = sphi_ctheta; \
+ RMAT_ELMT(_rm, 2, 0) = cphi_stheta_cpsi + sphi_spsi; \
+ RMAT_ELMT(_rm, 2, 1) = cphi_stheta_spsi - sphi_cpsi; \
+ RMAT_ELMT(_rm, 2, 2) = cphi_ctheta; \
+ \
}
-#define INT32_RMAT_OF_EULERS_312(_rm, _e) { \
- \
- int32_t sphi; \
- PPRZ_ITRIG_SIN(sphi, (_e).phi); \
- int32_t cphi; \
- PPRZ_ITRIG_COS(cphi, (_e).phi); \
- int32_t stheta; \
- PPRZ_ITRIG_SIN(stheta, (_e).theta); \
- int32_t ctheta; \
- PPRZ_ITRIG_COS(ctheta, (_e).theta); \
- int32_t spsi; \
- PPRZ_ITRIG_SIN(spsi, (_e).psi); \
- int32_t cpsi; \
- PPRZ_ITRIG_COS(cpsi, (_e).psi); \
- \
- \
+#define INT32_RMAT_OF_EULERS_312(_rm, _e) { \
+ \
+ int32_t sphi; \
+ PPRZ_ITRIG_SIN(sphi, (_e).phi); \
+ int32_t cphi; \
+ PPRZ_ITRIG_COS(cphi, (_e).phi); \
+ int32_t stheta; \
+ PPRZ_ITRIG_SIN(stheta, (_e).theta); \
+ int32_t ctheta; \
+ PPRZ_ITRIG_COS(ctheta, (_e).theta); \
+ int32_t spsi; \
+ PPRZ_ITRIG_SIN(spsi, (_e).psi); \
+ int32_t cpsi; \
+ PPRZ_ITRIG_COS(cpsi, (_e).psi); \
+ \
+ \
int32_t stheta_spsi = INT_MULT_RSHIFT(stheta, spsi, INT32_TRIG_FRAC); \
int32_t stheta_cpsi = INT_MULT_RSHIFT(stheta, cpsi, INT32_TRIG_FRAC); \
int32_t ctheta_spsi = INT_MULT_RSHIFT(ctheta, spsi, INT32_TRIG_FRAC); \
@@ -516,22 +482,22 @@ struct Int64Vect3 {
int32_t cphi_cpsi = INT_MULT_RSHIFT(cphi, cpsi, INT32_TRIG_FRAC); \
int32_t sphi_stheta = INT_MULT_RSHIFT(sphi, stheta, INT32_TRIG_FRAC); \
int32_t sphi_ctheta = INT_MULT_RSHIFT(sphi, ctheta, INT32_TRIG_FRAC); \
- \
+ \
int32_t sphi_stheta_spsi = INT_MULT_RSHIFT(sphi_stheta, spsi, INT32_TRIG_FRAC); \
int32_t sphi_stheta_cpsi = INT_MULT_RSHIFT(sphi_stheta, cpsi, INT32_TRIG_FRAC); \
int32_t sphi_ctheta_spsi = INT_MULT_RSHIFT(sphi_ctheta, spsi, INT32_TRIG_FRAC); \
int32_t sphi_ctheta_cpsi = INT_MULT_RSHIFT(sphi_ctheta, cpsi, INT32_TRIG_FRAC); \
- \
- RMAT_ELMT(_rm, 0, 0) = ctheta_cpsi - sphi_stheta_spsi; \
- RMAT_ELMT(_rm, 0, 1) = ctheta_spsi + sphi_stheta_cpsi; \
- RMAT_ELMT(_rm, 0, 2) = -cphi_stheta; \
- RMAT_ELMT(_rm, 1, 0) = -cphi_spsi; \
- RMAT_ELMT(_rm, 1, 1) = cphi_cpsi; \
- RMAT_ELMT(_rm, 1, 2) = sphi; \
- RMAT_ELMT(_rm, 2, 0) = stheta_cpsi + sphi_ctheta_spsi; \
- RMAT_ELMT(_rm, 2, 1) = stheta_spsi - sphi_ctheta_cpsi; \
- RMAT_ELMT(_rm, 2, 2) = cphi_ctheta; \
- \
+ \
+ RMAT_ELMT(_rm, 0, 0) = ctheta_cpsi - sphi_stheta_spsi; \
+ RMAT_ELMT(_rm, 0, 1) = ctheta_spsi + sphi_stheta_cpsi; \
+ RMAT_ELMT(_rm, 0, 2) = -cphi_stheta; \
+ RMAT_ELMT(_rm, 1, 0) = -cphi_spsi; \
+ RMAT_ELMT(_rm, 1, 1) = cphi_cpsi; \
+ RMAT_ELMT(_rm, 1, 2) = sphi; \
+ RMAT_ELMT(_rm, 2, 0) = stheta_cpsi + sphi_ctheta_spsi; \
+ RMAT_ELMT(_rm, 2, 1) = stheta_spsi - sphi_ctheta_cpsi; \
+ RMAT_ELMT(_rm, 2, 2) = cphi_ctheta; \
+ \
}
@@ -539,36 +505,38 @@ struct Int64Vect3 {
* Quaternions
*/
-#define INT32_QUAT_ZERO(_q) { \
- (_q).qi = QUAT1_BFP_OF_REAL(1); \
- (_q).qx = 0; \
- (_q).qy = 0; \
- (_q).qz = 0; \
+#define INT32_QUAT_ZERO(_q) { \
+ (_q).qi = QUAT1_BFP_OF_REAL(1); \
+ (_q).qx = 0; \
+ (_q).qy = 0; \
+ (_q).qz = 0; \
}
#define INT32_QUAT_INVERT(_qo, _qi) QUAT_INVERT(_qo, _qi)
-#define INT32_QUAT_NORM(n, q) { \
+#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_SQRT(n, n2); \
+ INT32_SQRT(n, n2); \
}
-#define INT32_QUAT_WRAP_SHORTEST(q) { \
- if ((q).qi < 0) \
- QUAT_EXPLEMENTARY(q,q); \
+#define INT32_QUAT_WRAP_SHORTEST(q) { \
+ if ((q).qi < 0) \
+ QUAT_EXPLEMENTARY(q,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; \
+#define INT32_QUAT_NORMALIZE(q) { \
+ int32_t n; \
+ INT32_QUAT_NORM(n, q); \
+ if (n > 0) { \
+ (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 */
-#define INT32_QUAT_COMP(_a2c, _a2b, _b2c) { \
+#define INT32_QUAT_COMP(_a2c, _a2b, _b2c) { \
(_a2c).qi = ((_a2b).qi*(_b2c).qi - (_a2b).qx*(_b2c).qx - (_a2b).qy*(_b2c).qy - (_a2b).qz*(_b2c).qz)>>INT32_QUAT_FRAC; \
(_a2c).qx = ((_a2b).qi*(_b2c).qx + (_a2b).qx*(_b2c).qi + (_a2b).qy*(_b2c).qz - (_a2b).qz*(_b2c).qy)>>INT32_QUAT_FRAC; \
(_a2c).qy = ((_a2b).qi*(_b2c).qy - (_a2b).qx*(_b2c).qz + (_a2b).qy*(_b2c).qi + (_a2b).qz*(_b2c).qx)>>INT32_QUAT_FRAC; \
@@ -576,7 +544,7 @@ struct Int64Vect3 {
}
/* _a2b = _a2b comp_inv _b2c , aka _a2b = inv(_b2c) * _a2c */
-#define INT32_QUAT_COMP_INV(_a2b, _a2c, _b2c) { \
+#define INT32_QUAT_COMP_INV(_a2b, _a2c, _b2c) { \
(_a2b).qi = ( (_a2c).qi*(_b2c).qi + (_a2c).qx*(_b2c).qx + (_a2c).qy*(_b2c).qy + (_a2c).qz*(_b2c).qz)>>INT32_QUAT_FRAC; \
(_a2b).qx = (-(_a2c).qi*(_b2c).qx + (_a2c).qx*(_b2c).qi - (_a2c).qy*(_b2c).qz + (_a2c).qz*(_b2c).qy)>>INT32_QUAT_FRAC; \
(_a2b).qy = (-(_a2c).qi*(_b2c).qy + (_a2c).qx*(_b2c).qz + (_a2c).qy*(_b2c).qi - (_a2c).qz*(_b2c).qx)>>INT32_QUAT_FRAC; \
@@ -584,7 +552,7 @@ struct Int64Vect3 {
}
/* _b2c = _a2b inv_comp _a2c , aka _b2c = _a2c * inv(_a2b) */
-#define INT32_QUAT_INV_COMP(_b2c, _a2b, _a2c) { \
+#define INT32_QUAT_INV_COMP(_b2c, _a2b, _a2c) { \
(_b2c).qi = ((_a2b).qi*(_a2c).qi + (_a2b).qx*(_a2c).qx + (_a2b).qy*(_a2c).qy + (_a2b).qz*(_a2c).qz)>>INT32_QUAT_FRAC; \
(_b2c).qx = ((_a2b).qi*(_a2c).qx - (_a2b).qx*(_a2c).qi - (_a2b).qy*(_a2c).qz + (_a2b).qz*(_a2c).qy)>>INT32_QUAT_FRAC; \
(_b2c).qy = ((_a2b).qi*(_a2c).qy + (_a2b).qx*(_a2c).qz - (_a2b).qy*(_a2c).qi - (_a2b).qz*(_a2c).qx)>>INT32_QUAT_FRAC; \
@@ -599,16 +567,16 @@ struct Int64Vect3 {
}
/* _a2c = _a2b comp _b2c , aka _a2c = _a2b * _b2c */
-#define INT32_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c) { \
- INT32_QUAT_COMP(_a2c, _a2b, _b2c); \
- INT32_QUAT_WRAP_SHORTEST(_a2c); \
- INT32_QUAT_NORMALIZE(_a2c); \
+#define INT32_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c) { \
+ INT32_QUAT_COMP(_a2c, _a2b, _b2c); \
+ INT32_QUAT_WRAP_SHORTEST(_a2c); \
+ INT32_QUAT_NORMALIZE(_a2c); \
}
/* _qd = -0.5*omega(_r) * _q */
// mult with 0.5 is done by shifting one more bit to the right
-#define INT32_QUAT_DERIVATIVE(_qd, _r, _q) { \
+#define INT32_QUAT_DERIVATIVE(_qd, _r, _q) { \
(_qd).qi = (-( (_r).p*(_q).qx + (_r).q*(_q).qy + (_r).r*(_q).qz))>>(INT32_RATE_FRAC+1); \
(_qd).qx = (-(-(_r).p*(_q).qi - (_r).r*(_q).qy + (_r).q*(_q).qz))>>(INT32_RATE_FRAC+1); \
(_qd).qy = (-(-(_r).q*(_q).qi + (_r).r*(_q).qx - (_r).p*(_q).qz))>>(INT32_RATE_FRAC+1); \
@@ -641,172 +609,145 @@ struct Int64Vect3 {
}
-#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 m00 = qi2 + qx2 - qy2 - qz2; \
- const int32_t m01 = 2 * (qxqy + qiqz ); \
- const int32_t m02 = 2 * (qxqz - qiqy ); \
- const int32_t m10 = 2 * (qxqy - qiqz ); \
- const int32_t m11 = qi2 - qx2 + qy2 - qz2; \
- const int32_t m12 = 2 * (qyqz + qiqx ); \
- 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; \
- }
-#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; \
+#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; \
}
-#endif
/*
* http://www.mathworks.com/access/helpdesk_r13/help/toolbox/aeroblks/euleranglestoquaternions.html
*/
-#define INT32_QUAT_OF_EULERS(_q, _e) { \
- const int32_t phi2 = (_e).phi / 2; \
- const int32_t theta2 = (_e).theta / 2; \
- const int32_t psi2 = (_e).psi / 2; \
- \
- int32_t s_phi2; \
- PPRZ_ITRIG_SIN(s_phi2, phi2); \
- int32_t c_phi2; \
- PPRZ_ITRIG_COS(c_phi2, phi2); \
- int32_t s_theta2; \
- PPRZ_ITRIG_SIN(s_theta2, theta2); \
- int32_t c_theta2; \
- PPRZ_ITRIG_COS(c_theta2, theta2); \
- int32_t s_psi2; \
- PPRZ_ITRIG_SIN(s_psi2, psi2); \
- int32_t c_psi2; \
- PPRZ_ITRIG_COS(c_psi2, psi2); \
- \
+#define INT32_QUAT_OF_EULERS(_q, _e) { \
+ const int32_t phi2 = (_e).phi / 2; \
+ const int32_t theta2 = (_e).theta / 2; \
+ const int32_t psi2 = (_e).psi / 2; \
+ \
+ int32_t s_phi2; \
+ PPRZ_ITRIG_SIN(s_phi2, phi2); \
+ int32_t c_phi2; \
+ PPRZ_ITRIG_COS(c_phi2, phi2); \
+ int32_t s_theta2; \
+ PPRZ_ITRIG_SIN(s_theta2, theta2); \
+ int32_t c_theta2; \
+ PPRZ_ITRIG_COS(c_theta2, theta2); \
+ int32_t s_psi2; \
+ PPRZ_ITRIG_SIN(s_psi2, psi2); \
+ int32_t c_psi2; \
+ PPRZ_ITRIG_COS(c_psi2, psi2); \
+ \
int32_t c_th_c_ps = INT_MULT_RSHIFT(c_theta2, c_psi2, INT32_TRIG_FRAC); \
int32_t c_th_s_ps = INT_MULT_RSHIFT(c_theta2, s_psi2, INT32_TRIG_FRAC); \
int32_t s_th_s_ps = INT_MULT_RSHIFT(s_theta2, s_psi2, INT32_TRIG_FRAC); \
int32_t s_th_c_ps = INT_MULT_RSHIFT(s_theta2, c_psi2, INT32_TRIG_FRAC); \
- \
+ \
(_q).qi = INT_MULT_RSHIFT( c_phi2, c_th_c_ps, INT32_TRIG_FRAC + INT32_TRIG_FRAC - INT32_QUAT_FRAC) + \
- INT_MULT_RSHIFT( s_phi2, s_th_s_ps, INT32_TRIG_FRAC + INT32_TRIG_FRAC - INT32_QUAT_FRAC); \
+ INT_MULT_RSHIFT( s_phi2, s_th_s_ps, INT32_TRIG_FRAC + INT32_TRIG_FRAC - INT32_QUAT_FRAC); \
(_q).qx = INT_MULT_RSHIFT(-c_phi2, s_th_s_ps, INT32_TRIG_FRAC + INT32_TRIG_FRAC - INT32_QUAT_FRAC) + \
- INT_MULT_RSHIFT( s_phi2, c_th_c_ps, INT32_TRIG_FRAC + INT32_TRIG_FRAC - INT32_QUAT_FRAC); \
+ INT_MULT_RSHIFT( s_phi2, c_th_c_ps, INT32_TRIG_FRAC + INT32_TRIG_FRAC - INT32_QUAT_FRAC); \
(_q).qy = INT_MULT_RSHIFT( c_phi2, s_th_c_ps, INT32_TRIG_FRAC + INT32_TRIG_FRAC - INT32_QUAT_FRAC) + \
- INT_MULT_RSHIFT( s_phi2, c_th_s_ps, INT32_TRIG_FRAC + INT32_TRIG_FRAC - INT32_QUAT_FRAC); \
+ INT_MULT_RSHIFT( s_phi2, c_th_s_ps, INT32_TRIG_FRAC + INT32_TRIG_FRAC - INT32_QUAT_FRAC); \
(_q).qz = INT_MULT_RSHIFT( c_phi2, c_th_s_ps, INT32_TRIG_FRAC + INT32_TRIG_FRAC - INT32_QUAT_FRAC) + \
- INT_MULT_RSHIFT(-s_phi2, s_th_c_ps, INT32_TRIG_FRAC + INT32_TRIG_FRAC - INT32_QUAT_FRAC); \
+ INT_MULT_RSHIFT(-s_phi2, s_th_c_ps, INT32_TRIG_FRAC + INT32_TRIG_FRAC - INT32_QUAT_FRAC); \
}
-#define INT32_QUAT_OF_AXIS_ANGLE(_q, _uv, _an) { \
- int32_t san2; \
- PPRZ_ITRIG_SIN(san2, (_an/2)); \
- int32_t can2; \
- PPRZ_ITRIG_COS(can2, (_an/2)); \
- _q.qi = can2; \
- _q.qx = san2 * _uv.x; \
- _q.qy = san2 * _uv.y; \
- _q.qz = san2 * _uv.z; \
+#define INT32_QUAT_OF_AXIS_ANGLE(_q, _uv, _an) { \
+ int32_t san2; \
+ PPRZ_ITRIG_SIN(san2, (_an/2)); \
+ int32_t can2; \
+ PPRZ_ITRIG_COS(can2, (_an/2)); \
+ _q.qi = can2; \
+ _q.qx = san2 * _uv.x; \
+ _q.qy = san2 * _uv.y; \
+ _q.qz = san2 * _uv.z; \
}
-#define INT32_QUAT_OF_RMAT(_q, _r) { \
- const int32_t tr = RMAT_TRACE(_r); \
- if (tr > 0) { \
- const int32_t two_qi_two = TRIG_BFP_OF_REAL(1.) + tr; \
- int32_t two_qi; \
- INT32_SQRT(two_qi, (two_qi_two< RMAT_ELMT(_r, 1, 1) && \
- RMAT_ELMT(_r, 0, 0) > RMAT_ELMT(_r, 2, 2)) { \
- const int32_t two_qx_two = RMAT_ELMT(_r, 0, 0) - RMAT_ELMT(_r, 1, 1) \
- - RMAT_ELMT(_r, 2, 2) + TRIG_BFP_OF_REAL(1.); \
- int32_t two_qx; \
- INT32_SQRT(two_qx, (two_qx_two< RMAT_ELMT(_r, 2, 2)) { \
- const int32_t two_qy_two = RMAT_ELMT(_r, 1, 1) - RMAT_ELMT(_r, 0, 0) \
- - RMAT_ELMT(_r, 2, 2) + TRIG_BFP_OF_REAL(1.); \
- int32_t two_qy; \
- INT32_SQRT(two_qy, (two_qy_two< 0) { \
+ const int32_t two_qi_two = TRIG_BFP_OF_REAL(1.) + tr; \
+ int32_t two_qi; \
+ INT32_SQRT(two_qi, (two_qi_two< RMAT_ELMT(_r, 1, 1) && \
+ RMAT_ELMT(_r, 0, 0) > RMAT_ELMT(_r, 2, 2)) { \
+ const int32_t two_qx_two = RMAT_ELMT(_r, 0, 0) - RMAT_ELMT(_r, 1, 1) \
+ - RMAT_ELMT(_r, 2, 2) + TRIG_BFP_OF_REAL(1.); \
+ int32_t two_qx; \
+ INT32_SQRT(two_qx, (two_qx_two< RMAT_ELMT(_r, 2, 2)) { \
+ const int32_t two_qy_two = RMAT_ELMT(_r, 1, 1) - RMAT_ELMT(_r, 0, 0) \
+ - RMAT_ELMT(_r, 2, 2) + TRIG_BFP_OF_REAL(1.); \
+ int32_t two_qy; \
+ INT32_SQRT(two_qy, (two_qy_two<> (_r)); \
- (_o).q = ((_i).q >> (_r)); \
- (_o).r = ((_i).r >> (_r)); \
+#define INT_RATES_RSHIFT(_o, _i, _r) { \
+ (_o).p = ((_i).p >> (_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 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); \
- int32_t cphi; \
- PPRZ_ITRIG_COS(cphi, (_e).phi); \
- int32_t stheta; \
- PPRZ_ITRIG_SIN(stheta, (_e).theta); \
- int32_t ctheta; \
- PPRZ_ITRIG_COS(ctheta, (_e).theta); \
- \
+#define INT32_RATES_OF_EULERS_DOT_321(_r, _e, _ed) { \
+ \
+ int32_t sphi; \
+ PPRZ_ITRIG_SIN(sphi, (_e).phi); \
+ int32_t cphi; \
+ PPRZ_ITRIG_COS(cphi, (_e).phi); \
+ int32_t stheta; \
+ PPRZ_ITRIG_SIN(stheta, (_e).theta); \
+ int32_t ctheta; \
+ PPRZ_ITRIG_COS(ctheta, (_e).theta); \
+ \
int32_t cphi_ctheta = INT_MULT_RSHIFT(cphi, ctheta, INT32_TRIG_FRAC); \
int32_t sphi_ctheta = INT_MULT_RSHIFT(sphi, ctheta, INT32_TRIG_FRAC); \
- \
+ \
(_r).p = - INT_MULT_RSHIFT(stheta, (_ed).psi, INT32_TRIG_FRAC) + (_ed).phi; \
(_r).q = INT_MULT_RSHIFT(sphi_ctheta, (_ed).psi, INT32_TRIG_FRAC) + INT_MULT_RSHIFT(cphi, (_ed).theta, INT32_TRIG_FRAC); \
(_r).r = INT_MULT_RSHIFT(cphi_ctheta, (_ed).psi, INT32_TRIG_FRAC) - INT_MULT_RSHIFT(sphi, (_ed).theta, INT32_TRIG_FRAC); \
- \
+ \
}
#define INT32_RATES_OF_EULERS_DOT(_r, _e, _ed) INT32_RATES_OF_EULERS_DOT_321(_r, _e, _ed)
-#define INT32_EULERS_DOT_321_OF_RATES(_ed, _e, _r) { \
- \
- int32_t sphi; \
- PPRZ_ITRIG_SIN(sphi, (_e).phi); \
- int32_t cphi; \
- PPRZ_ITRIG_COS(cphi, (_e).phi); \
- int32_t stheta; \
- PPRZ_ITRIG_SIN(stheta, (_e).theta); \
- int64_t ctheta; \
- PPRZ_ITRIG_COS(ctheta, (_e).theta); \
- \
- if (ctheta != 0) { \
+#define INT32_EULERS_DOT_321_OF_RATES(_ed, _e, _r) { \
+ \
+ int32_t sphi; \
+ PPRZ_ITRIG_SIN(sphi, (_e).phi); \
+ int32_t cphi; \
+ PPRZ_ITRIG_COS(cphi, (_e).phi); \
+ int32_t stheta; \
+ PPRZ_ITRIG_SIN(stheta, (_e).theta); \
+ int64_t ctheta; \
+ PPRZ_ITRIG_COS(ctheta, (_e).theta); \
+ \
+ if (ctheta != 0) { \
int64_t cphi_stheta = INT_MULT_RSHIFT(cphi, stheta, INT32_TRIG_FRAC); \
int64_t sphi_stheta = INT_MULT_RSHIFT(sphi, stheta, INT32_TRIG_FRAC); \
- \
+ \
(_ed).phi = (_r).p + (int32_t)((sphi_stheta * (int64_t)(_r).q) / ctheta) + (int32_t)((cphi_stheta * (int64_t)(_r).r) / ctheta); \
(_ed).theta = INT_MULT_RSHIFT(cphi, (_r).q, INT32_TRIG_FRAC) - INT_MULT_RSHIFT(sphi, (_r).r, INT32_TRIG_FRAC); \
(_ed).psi = (int32_t)(((int64_t)sphi * (int64_t)(_r).q) / ctheta) + (int32_t)(((int64_t)cphi * (int64_t)(_r).r) / ctheta); \
- } \
- /* FIXME: What do you wanna do when you hit the singularity ? */ \
- /* probably not return an uninitialized variable, or ? */ \
- else { \
- INT_EULERS_ZERO(_ed); \
- } \
+ } \
+ /* FIXME: What do you wanna do when you hit the singularity ? */ \
+ /* probably not return an uninitialized variable, or ? */ \
+ else { \
+ INT_EULERS_ZERO(_ed); \
+ } \
}
#define INT32_EULERS_DOT_OF_RATES(_ed, _e, _r) INT32_EULERS_DOT_321_OF_RATES(_ed, _e, _r)
@@ -977,23 +918,23 @@ struct Int64Vect3 {
*
*/
#define INT32_SQRT_MAX_ITER 40
-#define INT32_SQRT(_out,_in) { \
+#define INT32_SQRT(_out,_in) { \
if ((_in) == 0) \
(_out) = 0; \
- else { \
- uint32_t s1, s2; \
- uint8_t iter = 0; \
- s2 = _in; \
- do { \
- s1 = s2; \
- s2 = (_in) / s1; \
- s2 += s1; \
- s2 /= 2; \
- iter++; \
- } \
- while( ( (s1-s2) > 1) && (iter < INT32_SQRT_MAX_ITER)); \
- (_out) = s2; \
- } \
+ else { \
+ uint32_t s1, s2; \
+ uint8_t iter = 0; \
+ s2 = _in; \
+ do { \
+ s1 = s2; \
+ s2 = (_in) / s1; \
+ s2 += s1; \
+ s2 /= 2; \
+ iter++; \
+ } \
+ while( ( (s1-s2) > 1) && (iter < INT32_SQRT_MAX_ITER)); \
+ (_out) = s2; \
+ } \
}
@@ -1002,41 +943,41 @@ struct Int64Vect3 {
#define R_FRAC 14
-#define INT32_ATAN2(_a, _y, _x) { \
- const int32_t c1 = INT32_ANGLE_PI_4; \
- const int32_t c2 = 3 * INT32_ANGLE_PI_4; \
- const int32_t abs_y = abs(_y) + 1; \
- int32_t r; \
+#define INT32_ATAN2(_a, _y, _x) { \
+ const int32_t c1 = INT32_ANGLE_PI_4; \
+ 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); \
- } \
- else { \
+ } \
+ else { \
r = (((_x)+abs_y)<>R_FRAC); \
- } \
- if ((_y)<0) \
+ (_a) = c2 - ((c1 * r)>>R_FRAC); \
+ } \
+ if ((_y)<0) \
(_a) = -(_a); \
}
-#define INT32_ATAN2_2(_a, _y, _x) { \
- const int32_t c1 = INT32_ANGLE_PI_4; \
- 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; \
+#define INT32_ATAN2_2(_a, _y, _x) { \
+ const int32_t c1 = INT32_ANGLE_PI_4; \
+ 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; \
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; \
- } \
- else { \
- r = (((_x)+abs_y)<>R_FRAC); \
- } \
- if ((_y)<0) \
- (_a) = -(_a); \
+ (_a) = ((tmp1 * r)>>R_FRAC) + c1; \
+ } \
+ else { \
+ r = (((_x)+abs_y)<>R_FRAC); \
+ } \
+ if ((_y)<0) \
+ (_a) = -(_a); \
}
diff --git a/sw/airborne/math/pprz_geodetic_double.h b/sw/airborne/math/pprz_geodetic_double.h
index 69df6f8e44..5903506386 100644
--- a/sw/airborne/math/pprz_geodetic_double.h
+++ b/sw/airborne/math/pprz_geodetic_double.h
@@ -48,8 +48,6 @@ struct EcefCoor_d {
/**
* @brief vector in Latitude, Longitude and Altitude
- * @details Units lat,lon: radians
- * Unit alt: meters above MSL
*/
struct LlaCoor_d {
double lon; ///< in radians
diff --git a/sw/airborne/math/pprz_geodetic_float.h b/sw/airborne/math/pprz_geodetic_float.h
index 9d021880ad..7d964bf742 100644
--- a/sw/airborne/math/pprz_geodetic_float.h
+++ b/sw/airborne/math/pprz_geodetic_float.h
@@ -48,8 +48,6 @@ struct EcefCoor_f {
/**
* @brief vector in Latitude, Longitude and Altitude
- * @details Units lat,lon: radians
- * Unit alt: meters above MSL
*/
struct LlaCoor_f {
float lon; ///< in radians
diff --git a/sw/airborne/math/pprz_geodetic_int.c b/sw/airborne/math/pprz_geodetic_int.c
index 428472080c..d66e3cfe2a 100644
--- a/sw/airborne/math/pprz_geodetic_int.c
+++ b/sw/airborne/math/pprz_geodetic_int.c
@@ -24,6 +24,31 @@
#define HIGH_RES_TRIG_FRAC 20
+void ltp_of_ecef_rmat_from_lla_i(struct Int32Mat33* ltp_of_ecef, struct LlaCoor_i* lla) {
+
+#if USE_DOUBLE_PRECISION_TRIG
+ int32_t sin_lat = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)lla->lat)), HIGH_RES_TRIG_FRAC));
+ int32_t cos_lat = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)lla->lat)), HIGH_RES_TRIG_FRAC));
+ int32_t sin_lon = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)lla->lon)), HIGH_RES_TRIG_FRAC));
+ int32_t cos_lon = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)lla->lon)), HIGH_RES_TRIG_FRAC));
+#else
+ int32_t sin_lat = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)lla->lat)), HIGH_RES_TRIG_FRAC));
+ int32_t cos_lat = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)lla->lat)), HIGH_RES_TRIG_FRAC));
+ int32_t sin_lon = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)lla->lon)), HIGH_RES_TRIG_FRAC));
+ int32_t cos_lon = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)lla->lon)), HIGH_RES_TRIG_FRAC));
+#endif
+
+ ltp_of_ecef->m[0] = -sin_lon;
+ ltp_of_ecef->m[1] = cos_lon;
+ ltp_of_ecef->m[2] = 0; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */
+ ltp_of_ecef->m[3] = (int32_t)((-(int64_t)sin_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC);
+ ltp_of_ecef->m[4] = (int32_t)((-(int64_t)sin_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC);
+ ltp_of_ecef->m[5] = cos_lat;
+ ltp_of_ecef->m[6] = (int32_t)(( (int64_t)cos_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC);
+ ltp_of_ecef->m[7] = (int32_t)(( (int64_t)cos_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC);
+ ltp_of_ecef->m[8] = sin_lat;
+}
+
void ltp_def_from_ecef_i(struct LtpDef_i* def, struct EcefCoor_i* ecef) {
/* store the origin of the tangeant plane */
@@ -31,29 +56,7 @@ void ltp_def_from_ecef_i(struct LtpDef_i* def, struct EcefCoor_i* ecef) {
/* compute the lla representation of the origin */
lla_of_ecef_i(&def->lla, &def->ecef);
/* 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;
+ ltp_of_ecef_rmat_from_lla_i(&def->ltp_of_ecef, &def->lla);
}
@@ -64,29 +67,7 @@ void ltp_def_from_lla_i(struct LtpDef_i* def, struct LlaCoor_i* 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;
+ ltp_of_ecef_rmat_from_lla_i(&def->ltp_of_ecef, &def->lla);
}
diff --git a/sw/airborne/math/pprz_geodetic_int.h b/sw/airborne/math/pprz_geodetic_int.h
index 9d95d52319..f17ff2dc5a 100644
--- a/sw/airborne/math/pprz_geodetic_int.h
+++ b/sw/airborne/math/pprz_geodetic_int.h
@@ -50,8 +50,6 @@ struct EcefCoor_i {
/**
* @brief vector in Latitude, Longitude and Altitude
- * @details Units lat,lon: radians*1e7
- * Unit alt: centimeters above MSL
*/
struct LlaCoor_i {
int32_t lon; ///< in radians*1e7
@@ -99,6 +97,7 @@ struct LtpDef_i {
int32_t hmsl; ///< Height above mean sea level in mm
};
+extern void ltp_of_ecef_rmat_from_lla_i(struct Int32Mat33* ltp_of_ecef, struct LlaCoor_i* lla);
extern void ltp_def_from_ecef_i(struct LtpDef_i* def, struct EcefCoor_i* ecef);
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);
diff --git a/sw/airborne/math/pprz_simple_matrix.h b/sw/airborne/math/pprz_simple_matrix.h
index 4b6fd9be8b..b74aa6dd26 100644
--- a/sw/airborne/math/pprz_simple_matrix.h
+++ b/sw/airborne/math/pprz_simple_matrix.h
@@ -13,38 +13,38 @@
//
// C = A*B A:(i,k) B:(k,j) C:(i,j)
//
-#define MAT_MUL(_i, _k, _j, C, A, B) { \
- int l,c,m; \
- for (l=0; l<_i; l++) \
- for (c=0; c<_j; c++) { \
- C[l][c] = 0.; \
- for (m=0; m<_k; m++) \
- C[l][c] += A[l][m]*B[m][c]; \
- } \
+#define MAT_MUL(_i, _k, _j, C, A, B) { \
+ int l,c,m; \
+ for (l=0; l<_i; l++) \
+ for (c=0; c<_j; c++) { \
+ C[l][c] = 0.; \
+ for (m=0; m<_k; m++) \
+ C[l][c] += A[l][m]*B[m][c]; \
+ } \
}
//
// C = A*B' A:(i,k) B:(j,k) C:(i,j)
//
-#define MAT_MUL_T(_i, _k, _j, C, A, B) { \
- int l,c,m; \
- for (l=0; l<_i; l++) \
- for (c=0; c<_j; c++) { \
- C[l][c] = 0.; \
- for (m=0; m<_k; m++) \
- C[l][c] += A[l][m]*B[c][m]; \
- } \
+#define MAT_MUL_T(_i, _k, _j, C, A, B) { \
+ int l,c,m; \
+ for (l=0; l<_i; l++) \
+ for (c=0; c<_j; c++) { \
+ C[l][c] = 0.; \
+ for (m=0; m<_k; m++) \
+ C[l][c] += A[l][m]*B[c][m]; \
+ } \
}
//
// C = A-B
//
-#define MAT_SUB(_i, _j, C, A, B) { \
- int l,c; \
- for (l=0; l<_i; l++) \
- for (c=0; c<_j; c++) \
- C[l][c] = A[l][c] - B[l][c]; \
+#define MAT_SUB(_i, _j, C, A, B) { \
+ int l,c; \
+ for (l=0; l<_i; l++) \
+ for (c=0; c<_j; c++) \
+ C[l][c] = A[l][c] - B[l][c]; \
}
@@ -53,31 +53,31 @@
//
// invS = 1/det(S) com(S)'
//
-#define MAT_INV33(_invS, _S) { \
- const float m00 = _S[1][1]*_S[2][2] - _S[1][2]*_S[2][1]; \
- const float m10 = _S[0][1]*_S[2][2] - _S[0][2]*_S[2][1]; \
- const float m20 = _S[0][1]*_S[1][2] - _S[0][2]*_S[1][1]; \
- const float m01 = _S[1][0]*_S[2][2] - _S[1][2]*_S[2][0]; \
- const float m11 = _S[0][0]*_S[2][2] - _S[0][2]*_S[2][0]; \
- const float m21 = _S[0][0]*_S[1][2] - _S[0][2]*_S[1][0]; \
- const float m02 = _S[1][0]*_S[2][1] - _S[1][1]*_S[2][0]; \
- const float m12 = _S[0][0]*_S[2][1] - _S[0][1]*_S[2][0]; \
- const float m22 = _S[0][0]*_S[1][1] - _S[0][1]*_S[1][0]; \
- float det = _S[0][0]*m00 - _S[1][0]*m10 + _S[2][0]*m20; \
- if (fabs(det) < FLT_EPSILON) { \
+#define MAT_INV33(_invS, _S) { \
+ const float m00 = _S[1][1]*_S[2][2] - _S[1][2]*_S[2][1]; \
+ const float m10 = _S[0][1]*_S[2][2] - _S[0][2]*_S[2][1]; \
+ const float m20 = _S[0][1]*_S[1][2] - _S[0][2]*_S[1][1]; \
+ const float m01 = _S[1][0]*_S[2][2] - _S[1][2]*_S[2][0]; \
+ const float m11 = _S[0][0]*_S[2][2] - _S[0][2]*_S[2][0]; \
+ const float m21 = _S[0][0]*_S[1][2] - _S[0][2]*_S[1][0]; \
+ const float m02 = _S[1][0]*_S[2][1] - _S[1][1]*_S[2][0]; \
+ const float m12 = _S[0][0]*_S[2][1] - _S[0][1]*_S[2][0]; \
+ const float m22 = _S[0][0]*_S[1][1] - _S[0][1]*_S[1][0]; \
+ float det = _S[0][0]*m00 - _S[1][0]*m10 + _S[2][0]*m20; \
+ if (fabs(det) < FLT_EPSILON) { \
/* If the determinant is too small then set it to epsilon preserving sign. */ \
warn_message("warning: %s:%d MAT_INV33 trying to invert non-invertable matrix '%s' and put result in '%s'.\n", __FILE__, __LINE__, #_S, #_invS); \
- det = copysignf(FLT_EPSILON, det); \
- } \
- _invS[0][0] = m00 / det; \
- _invS[1][0] = -m01 / det; \
- _invS[2][0] = m02 / det; \
- _invS[0][1] = -m10 / det; \
- _invS[1][1] = m11 / det; \
- _invS[2][1] = -m12 / det; \
- _invS[0][2] = m20 / det; \
- _invS[1][2] = -m21 / det; \
- _invS[2][2] = m22 / det; \
+ det = copysignf(FLT_EPSILON, det); \
+ } \
+ _invS[0][0] = m00 / det; \
+ _invS[1][0] = -m01 / det; \
+ _invS[2][0] = m02 / det; \
+ _invS[0][1] = -m10 / det; \
+ _invS[1][1] = m11 / det; \
+ _invS[2][1] = -m12 / det; \
+ _invS[0][2] = m20 / det; \
+ _invS[1][2] = -m21 / det; \
+ _invS[2][2] = m22 / det; \
}
diff --git a/sw/airborne/mcu.c b/sw/airborne/mcu.c
index c0472a7d59..53c99dc2e1 100644
--- a/sw/airborne/mcu.c
+++ b/sw/airborne/mcu.c
@@ -101,6 +101,9 @@ void mcu_init(void) {
#ifdef USE_I2C2
i2c2_init();
#endif
+#ifdef USE_I2C3
+ i2c3_init();
+#endif
#ifdef USE_ADC
adc_init();
#endif
@@ -140,6 +143,10 @@ void mcu_init(void) {
spi3_slave_init();
#endif
#endif // SPI_SLAVE
+
+#if SPI_SLAVE_HS
+ spi_slave_hs_init();
+#endif
#endif // USE_SPI
#ifdef USE_DAC
diff --git a/sw/airborne/mcu_periph/gpio.h b/sw/airborne/mcu_periph/gpio.h
index 1218ae6c05..0c500b3dd6 100644
--- a/sw/airborne/mcu_periph/gpio.h
+++ b/sw/airborne/mcu_periph/gpio.h
@@ -25,6 +25,9 @@
* Some architecture independent helper functions for GPIOs.
*/
+#ifndef MCU_PERIPH_GPIO_H
+#define MCU_PERIPH_GPIO_H
+
#include "std.h"
#include "mcu_periph/gpio_arch.h"
@@ -38,3 +41,4 @@ extern void gpio_setup_output(uint32_t port, uint16_t pin);
*/
extern void gpio_setup_input(uint32_t port, uint16_t pin);
+#endif /* MCU_PERIPH_GPIO_H */
diff --git a/sw/airborne/mcu_periph/i2c.c b/sw/airborne/mcu_periph/i2c.c
index c30e06bd56..95a88c2541 100644
--- a/sw/airborne/mcu_periph/i2c.c
+++ b/sw/airborne/mcu_periph/i2c.c
@@ -37,6 +37,7 @@ struct i2c_periph i2c0;
#if DOWNLINK
static void send_i2c0_err(void) {
+ uint16_t i2c0_queue_full_cnt = i2c0.errors->queue_full_cnt;
uint16_t i2c0_ack_fail_cnt = i2c0.errors->ack_fail_cnt;
uint16_t i2c0_miss_start_stop_cnt = i2c0.errors->miss_start_stop_cnt;
uint16_t i2c0_arb_lost_cnt = i2c0.errors->arb_lost_cnt;
@@ -48,6 +49,7 @@ static void send_i2c0_err(void) {
uint32_t i2c0_last_unexpected_event = i2c0.errors->last_unexpected_event;
const uint8_t _bus0 = 0;
DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice,
+ $i2c0_queue_full_cnt,
&i2c0_ack_fail_cnt,
&i2c0_miss_start_stop_cnt,
&i2c0_arb_lost_cnt,
@@ -75,6 +77,7 @@ struct i2c_periph i2c1;
#if DOWNLINK
static void send_i2c1_err(void) {
+ uint16_t i2c1_queue_full_cnt = i2c1.errors->queue_full_cnt;
uint16_t i2c1_ack_fail_cnt = i2c1.errors->ack_fail_cnt;
uint16_t i2c1_miss_start_stop_cnt = i2c1.errors->miss_start_stop_cnt;
uint16_t i2c1_arb_lost_cnt = i2c1.errors->arb_lost_cnt;
@@ -86,6 +89,7 @@ static void send_i2c1_err(void) {
uint32_t i2c1_last_unexpected_event = i2c1.errors->last_unexpected_event;
const uint8_t _bus1 = 1;
DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice,
+ $i2c1_queue_full_cnt,
&i2c1_ack_fail_cnt,
&i2c1_miss_start_stop_cnt,
&i2c1_arb_lost_cnt,
@@ -113,6 +117,7 @@ struct i2c_periph i2c2;
#if DOWNLINK
static void send_i2c2_err(void) {
+ uint16_t i2c2_queue_full_cnt = i2c2.errors->queue_full_cnt;
uint16_t i2c2_ack_fail_cnt = i2c2.errors->ack_fail_cnt;
uint16_t i2c2_miss_start_stop_cnt = i2c2.errors->miss_start_stop_cnt;
uint16_t i2c2_arb_lost_cnt = i2c2.errors->arb_lost_cnt;
@@ -124,6 +129,7 @@ static void send_i2c2_err(void) {
uint32_t i2c2_last_unexpected_event = i2c2.errors->last_unexpected_event;
const uint8_t _bus2 = 2;
DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice,
+ $i2c2_queue_full_cnt,
&i2c2_ack_fail_cnt,
&i2c2_miss_start_stop_cnt,
&i2c2_arb_lost_cnt,
@@ -144,6 +150,45 @@ void i2c2_init(void) {
#endif /* USE_I2C2 */
+#ifdef USE_I2C3
+
+struct i2c_periph i2c3;
+
+void i2c3_init(void) {
+ i2c_init(&i2c3);
+ i2c3_hw_init();
+}
+
+#if DOWNLINK
+static void send_i2c3_err(void) {
+ uint16_t i2c3_queue_full_cnt = i2c3.errors->queue_full_cnt;
+ uint16_t i2c3_ack_fail_cnt = i2c3.errors->ack_fail_cnt;
+ uint16_t i2c3_miss_start_stop_cnt = i2c3.errors->miss_start_stop_cnt;
+ uint16_t i2c3_arb_lost_cnt = i2c3.errors->arb_lost_cnt;
+ uint16_t i2c3_over_under_cnt = i2c3.errors->over_under_cnt;
+ uint16_t i2c3_pec_recep_cnt = i2c3.errors->pec_recep_cnt;
+ uint16_t i2c3_timeout_tlow_cnt = i2c3.errors->timeout_tlow_cnt;
+ uint16_t i2c3_smbus_alert_cnt = i2c3.errors->smbus_alert_cnt;
+ uint16_t i2c3_unexpected_event_cnt = i2c3.errors->unexpected_event_cnt;
+ uint32_t i2c3_last_unexpected_event = i2c3.errors->last_unexpected_event;
+ const uint8_t _bus3 = 3;
+ DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice,
+ $i2c3_queue_full_cnt,
+ &i2c3_ack_fail_cnt,
+ &i2c3_miss_start_stop_cnt,
+ &i2c3_arb_lost_cnt,
+ &i2c3_over_under_cnt,
+ &i2c3_pec_recep_cnt,
+ &i2c3_timeout_tlow_cnt,
+ &i2c3_smbus_alert_cnt,
+ &i2c3_unexpected_event_cnt,
+ &i2c3_last_unexpected_event,
+ &_bus3);
+}
+#endif
+
+#endif /* USE_I2C3 */
+
#if DOWNLINK
static void send_i2c_err(void) {
static uint8_t _i2c_nb_cnt = 0;
@@ -161,6 +206,11 @@ static void send_i2c_err(void) {
case 2:
#if USE_I2C2
send_i2c2_err();
+#endif
+ break;
+ case 3:
+#if USE_I2C3
+ send_i2c3_err();
#endif
break;
default:
@@ -172,6 +222,7 @@ static void send_i2c_err(void) {
}
#endif
+
void i2c_init(struct i2c_periph* p) {
p->trans_insert_idx = 0;
p->trans_extract_idx = 0;
diff --git a/sw/airborne/mcu_periph/i2c.h b/sw/airborne/mcu_periph/i2c.h
index ac8872bd9c..5d0220ebda 100644
--- a/sw/airborne/mcu_periph/i2c.h
+++ b/sw/airborne/mcu_periph/i2c.h
@@ -102,6 +102,7 @@ struct i2c_periph {
struct i2c_errors {
+ volatile uint16_t queue_full_cnt;
volatile uint16_t ack_fail_cnt;
volatile uint16_t miss_start_stop_cnt;
volatile uint16_t arb_lost_cnt;
@@ -126,6 +127,7 @@ struct i2c_errors {
}
#define ZEROS_ERR_COUNTER(_i2c_err) { \
+ _i2c_err.queue_full_cnt = 0; \
_i2c_err.ack_fail_cnt = 0; \
_i2c_err.miss_start_stop_cnt = 0; \
_i2c_err.arb_lost_cnt = 0; \
diff --git a/sw/airborne/mcu_periph/spi.h b/sw/airborne/mcu_periph/spi.h
index fb5c1fe011..60769eaeb4 100644
--- a/sw/airborne/mcu_periph/spi.h
+++ b/sw/airborne/mcu_periph/spi.h
@@ -359,4 +359,8 @@ extern bool_t spi_slave_wait(struct spi_periph* p);
/** @}*/
/** @}*/
+#if SPI_SLAVE_HS
+extern void spi_slave_hs_init(void);
+#endif
+
#endif /* SPI_H */
diff --git a/sw/airborne/mcu_periph/uart.h b/sw/airborne/mcu_periph/uart.h
index 84fc1626a6..13c1af34a6 100644
--- a/sw/airborne/mcu_periph/uart.h
+++ b/sw/airborne/mcu_periph/uart.h
@@ -36,20 +36,8 @@
#define UART_DEV_NAME_SIZE 16
/*
- * UART Baud rates
- * defines because the stupid c preprocessor can't handle enums
-*/
-#define B1200 1200
-#define B2400 2400
-#define B4800 4800
-#define B9600 9600
-#define B19200 19200
-#define B38400 38400
-#define B57600 57600
-#define B100000 100000
-#define B115200 115200
-#define B230400 230400
-#define B921600 921600
+ * UART Baud rate defines in arch/x/mcu_periph/uart_arch.h
+ */
#define UBITS_7 7
#define UBITS_8 8
diff --git a/sw/airborne/modules/bat_checker/bat_checker.c b/sw/airborne/modules/bat_checker/bat_checker.c
index 7949d8bd9b..bdc716cb4d 100644
--- a/sw/airborne/modules/bat_checker/bat_checker.c
+++ b/sw/airborne/modules/bat_checker/bat_checker.c
@@ -19,34 +19,24 @@
* Boston, MA 02111-1307, USA.
*/
+/**
+ * @file modules/bat_checker/bat_checker.c
+ *
+ * Activate a buzzer/LED periodically or periodically to warn of low/critical battery level.
+ * At LOW_BAT_LEVEL the buzzer will be activated periodically.
+ * At CRITIC_BAT_LEVEL the buzzer will be activated permanently.
+ */
+
#include "bat_checker.h"
#include "generated/airframe.h"
#include "generated/modules.h"
#include "subsystems/electrical.h"
#include "led.h"
-#ifndef CRITIC_BAT_LEVEL
-#error You must define CRITIC_BAT_LEVEL to use this module!
-#endif
-
-#ifndef LOW_BAT_LEVEL
-#error You must define LOW_BAT_LEVEL to use this module!
-#endif
-
#ifndef BAT_CHECKER_LED
#error You must define BAT_CHECKER_LED in your airframe file.
#endif
-#ifndef BAT_CHECKER_DELAY
-#define BAT_CHECKER_DELAY 5
-#endif
-PRINT_CONFIG_VAR(BAT_CHECKER_DELAY)
-
-// at this level, the buzzer will be activated periodically
-#define WARN_BAT_LEVEL1 (LOW_BAT_LEVEL*10)
-
-// at this level, the buzzer will be activated permanently
-#define WARN_BAT_LEVEL2 (CRITIC_BAT_LEVEL*10)
void init_bat_checker(void) {
LED_INIT(BAT_CHECKER_LED);
@@ -54,22 +44,12 @@ void init_bat_checker(void) {
}
void bat_checker_periodic(void) {
- static uint8_t bat_low_counter = 0;
- if(electrical.vsupply < WARN_BAT_LEVEL1) {
- if(bat_low_counter)
- bat_low_counter--;
- } else {
- bat_low_counter = BAT_CHECKER_DELAY * BAT_CHECKER_PERIODIC_FREQ;
- }
- if(!bat_low_counter) {
- if(electrical.vsupply < WARN_BAT_LEVEL2) {
- LED_ON(BAT_CHECKER_LED);
- } else if(electrical.vsupply < WARN_BAT_LEVEL1) {
- LED_TOGGLE(BAT_CHECKER_LED);
- }
- } else {
+ if (electrical.bat_critical)
+ LED_ON(BAT_CHECKER_LED);
+ else if (electrical.bat_low)
+ LED_TOGGLE(BAT_CHECKER_LED);
+ else
LED_OFF(BAT_CHECKER_LED);
- }
-}
+}
diff --git a/sw/airborne/modules/bat_checker/bat_checker.h b/sw/airborne/modules/bat_checker/bat_checker.h
index 77c07fe41d..5a3304e9fa 100644
--- a/sw/airborne/modules/bat_checker/bat_checker.h
+++ b/sw/airborne/modules/bat_checker/bat_checker.h
@@ -19,6 +19,14 @@
* Boston, MA 02111-1307, USA.
*/
+/**
+ * @file modules/bat_checker/bat_checker.c
+ *
+ * Activate a buzzer/LED periodically or periodically to warn of low/critical battery level.
+ * At LOW_BAT_LEVEL the buzzer will be activated periodically.
+ * At CRITIC_BAT_LEVEL the buzzer will be activated permanently.
+ */
+
#ifndef BAT_CHECKER_H
#define BAT_CHECKER_H
diff --git a/sw/airborne/modules/core/sys_mon.c b/sw/airborne/modules/core/sys_mon.c
index c999a0b0b0..d3e11d92cb 100644
--- a/sw/airborne/modules/core/sys_mon.c
+++ b/sw/airborne/modules/core/sys_mon.c
@@ -27,24 +27,26 @@
#include "mcu_periph/usb_serial.h"
#endif
-/* Global system monitor data (averaged over 1 sec) */
+/** Global system monitor data (averaged over 1 sec) */
struct SysMon sys_mon;
/* Local vars */
-uint16_t n_periodic;
-uint16_t n_event;
-uint32_t periodic_timer;
-uint32_t periodic_cycle;
-uint32_t event_timer;
-uint32_t sum_time_periodic; ///< in usec
-uint32_t sum_cycle_periodic; ///< in usec
-uint32_t sum_time_event; ///< in usec
-uint32_t min_time_event; ///< in usec
-uint32_t sum_n_event;
+static uint16_t n_periodic;
+static uint16_t n_event;
+static uint32_t periodic_timer;
+static uint32_t periodic_cycle;
+static uint32_t event_timer;
+static uint32_t sum_time_periodic; ///< in usec
+static uint32_t sum_cycle_periodic; ///< in usec
+static uint32_t sum_time_event; ///< in usec
+static uint32_t min_time_event; ///< in usec
+static uint32_t sum_n_event;
void init_sysmon(void) {
sys_mon.cpu_load = 0;
sys_mon.periodic_time = 0;
+ sys_mon.periodic_time_min = 0xFFFF;
+ sys_mon.periodic_time_max = 0;
sys_mon.periodic_cycle = 0;
sys_mon.periodic_cycle_min = 0xFFFF;
sys_mon.periodic_cycle_max = 0;
@@ -72,13 +74,19 @@ void periodic_report_sysmon(void) {
sys_mon.cpu_load = 100 * sys_mon.periodic_cycle / sys_mon.periodic_time;
sys_mon.event_number = sum_n_event / n_periodic;
- DOWNLINK_SEND_SYS_MON(DefaultChannel, DefaultDevice, &sys_mon.periodic_time, &sys_mon.periodic_cycle, &sys_mon.periodic_cycle_min, &sys_mon.periodic_cycle_max, &sys_mon.event_number, &sys_mon.cpu_load);
+ DOWNLINK_SEND_SYS_MON(DefaultChannel, DefaultDevice, &sys_mon.periodic_time,
+ &sys_mon.periodic_time_min, &sys_mon.periodic_time_max,
+ &sys_mon.periodic_cycle, &sys_mon.periodic_cycle_min,
+ &sys_mon.periodic_cycle_max, &sys_mon.event_number,
+ &sys_mon.cpu_load);
}
n_periodic = 0;
sum_time_periodic = 0;
sum_cycle_periodic = 0;
sum_n_event = 0;
+ sys_mon.periodic_time_min = 0xFFFF;
+ sys_mon.periodic_time_max = 0;
sys_mon.periodic_cycle_min = 0xFFFF;
sys_mon.periodic_cycle_max = 0;
}
@@ -93,6 +101,12 @@ void periodic_sysmon(void) {
periodic_cycle = periodic_usec - n_event * min_time_event;
sum_cycle_periodic += periodic_cycle;
+ /* remember min and max periodic times */
+ if (periodic_usec < sys_mon.periodic_time_min)
+ sys_mon.periodic_time_min = periodic_usec;
+ if (periodic_usec > sys_mon.periodic_time_max)
+ sys_mon.periodic_time_max = periodic_usec;
+
/* remember min and max periodic cycle times */
if (periodic_cycle < sys_mon.periodic_cycle_min)
sys_mon.periodic_cycle_min = periodic_cycle;
diff --git a/sw/airborne/modules/core/sys_mon.h b/sw/airborne/modules/core/sys_mon.h
index 0496f9b970..aec9286d7b 100644
--- a/sw/airborne/modules/core/sys_mon.h
+++ b/sw/airborne/modules/core/sys_mon.h
@@ -34,6 +34,8 @@
struct SysMon {
uint8_t cpu_load;
uint16_t periodic_time; ///< in usec
+ uint16_t periodic_time_min; ///< in usec
+ uint16_t periodic_time_max; ///< in usec
uint16_t periodic_cycle; ///< in usec
uint16_t periodic_cycle_min; ///< in usec
uint16_t periodic_cycle_max; ///< in usec
diff --git a/sw/airborne/modules/geo_mag/geo_mag.c b/sw/airborne/modules/geo_mag/geo_mag.c
index a615722137..aa5f60a105 100644
--- a/sw/airborne/modules/geo_mag/geo_mag.c
+++ b/sw/airborne/modules/geo_mag/geo_mag.c
@@ -31,8 +31,7 @@
#include "subsystems/gps.h"
#include "autopilot.h"
-// in int_cmpl_quat implementation, mag_h is an Int32Vect3 with INT32_MAG_FRAC
-#include "subsystems/ahrs/ahrs_int_cmpl_quat.h"
+#include "subsystems/ahrs.h"
bool_t geo_mag_calc_flag;
struct GeoMagVect geo_mag_vect;
@@ -71,8 +70,13 @@ void geo_mag_event(void) {
IEXT, EXT_COEFF1, EXT_COEFF2, EXT_COEFF3);
FLOAT_VECT3_NORMALIZE(geo_mag_vect);
+ // copy to ahrs
+#ifdef AHRS_FLOAT
+ VECT3_COPY(ahrs_impl.mag_h, geo_mag_vect);
+#else
// convert to MAG_BFP and copy to ahrs
VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(geo_mag_vect.x), MAG_BFP_OF_REAL(geo_mag_vect.y), MAG_BFP_OF_REAL(geo_mag_vect.z));
+#endif
geo_mag_vect.ready = TRUE;
}
diff --git a/sw/airborne/modules/gumstix_interface/qr_code_spi_link.c b/sw/airborne/modules/gumstix_interface/qr_code_spi_link.c
new file mode 100644
index 0000000000..c0a1a92526
--- /dev/null
+++ b/sw/airborne/modules/gumstix_interface/qr_code_spi_link.c
@@ -0,0 +1,72 @@
+/*
+ * Copyright (C) 2005-2013 The Paparazzi Team
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+#include "qr_code_spi_link.h"
+
+#include "subsystems/imu.h"
+#include "mcu_periph/spi.h"
+
+#include "mcu_periph/uart.h"
+#include "messages.h"
+#include "subsystems/datalink/downlink.h"
+
+//struct qr_code_spi_link_data qr_code_spi_link_data;
+struct spi_transaction qr_code_spi_link_transaction;
+
+static volatile bool_t qr_code_spi_data_available = FALSE;
+
+uint8_t testDataOut[3] = {1,2,3};
+uint8_t testDataIn[3] = {9,9,9};
+
+static void qr_code_spi_link_trans_cb( struct spi_transaction *trans );
+
+void qr_code_spi_link_init(void) {
+ qr_code_spi_link_transaction.cpol = SPICpolIdleHigh;
+ qr_code_spi_link_transaction.cpha = SPICphaEdge2;
+ qr_code_spi_link_transaction.dss = SPIDss8bit;
+ qr_code_spi_link_transaction.bitorder = SPIMSBFirst;
+ qr_code_spi_link_transaction.output_length = 3;
+ qr_code_spi_link_transaction.output_buf = testDataOut;
+ qr_code_spi_link_transaction.input_length = 3;
+ qr_code_spi_link_transaction.input_buf = testDataIn;
+ qr_code_spi_link_transaction.after_cb = qr_code_spi_link_trans_cb;
+ //spi_slave_set_config(&spi1, &qr_code_spi_link_transaction);
+ spi_slave_register(&spi1, &qr_code_spi_link_transaction);
+}
+
+
+void qr_code_spi_link_periodic(void) {
+ if (qr_code_spi_data_available) {
+ qr_code_spi_data_available = FALSE;
+ uint16_t x,y;
+ memcpy(&x,qr_code_spi_link_transaction.input_buf,2);
+ memcpy(&y,qr_code_spi_link_transaction.input_buf+2,2);
+ DOWNLINK_SEND_VISUALTARGET(DefaultChannel, DefaultDevice, &x, &y);
+ spi_slave_register(&spi1, &qr_code_spi_link_transaction);
+ }
+}
+
+static void qr_code_spi_link_trans_cb( struct spi_transaction *trans __attribute__ ((unused)) ) {
+ qr_code_spi_data_available = TRUE;
+}
+
+
diff --git a/sw/airborne/modules/gumstix_interface/qr_code_spi_link.h b/sw/airborne/modules/gumstix_interface/qr_code_spi_link.h
new file mode 100644
index 0000000000..06aa237943
--- /dev/null
+++ b/sw/airborne/modules/gumstix_interface/qr_code_spi_link.h
@@ -0,0 +1,31 @@
+/*
+ * Copyright (C) 2005-2013 The Paparazzi Team
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+#ifndef QR_CODE_SPI_LINK_H_
+#define QR_CODE_SPI_LINK_H_
+
+#include "std.h"
+
+extern void qr_code_spi_link_init(void);
+extern void qr_code_spi_link_periodic(void);
+
+#endif /* QR_CODE_SPI_LINK_H_ */
diff --git a/sw/airborne/modules/ins/imu_chimu.c b/sw/airborne/modules/ins/imu_chimu.c
index 4699a4f6d7..bc2400c460 100644
--- a/sw/airborne/modules/ins/imu_chimu.c
+++ b/sw/airborne/modules/ins/imu_chimu.c
@@ -149,7 +149,7 @@ void CHIMU_Init(CHIMU_PARSER_DATA *pstData)
unsigned char CHIMU_Parse(
unsigned char btData, /* input byte stream buffer */
- unsigned char bInputType, /* for future use if special builds of CHIMU data are performed */
+ unsigned char bInputType __attribute__((unused)), /* for future use if special builds of CHIMU data are performed */
CHIMU_PARSER_DATA *pstData) /* resulting data */
{
@@ -287,7 +287,7 @@ static unsigned char BitTest (unsigned char input, unsigned char n)
//Test a bit in n and return TRUE or FALSE
if ( input & (1 << n)) return TRUE; else return FALSE;
}
-unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID, unsigned char *pPayloadData, CHIMU_PARSER_DATA *pstData)
+unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID __attribute__((unused)), unsigned char *pPayloadData, CHIMU_PARSER_DATA *pstData)
{
//Msgs from CHIMU are off limits (i.e.any CHIMU messages sent up the uplink should go to
//CHIMU).
diff --git a/sw/airborne/modules/max7456/max7456.c b/sw/airborne/modules/max7456/max7456.c
new file mode 100644
index 0000000000..500aa20f72
--- /dev/null
+++ b/sw/airborne/modules/max7456/max7456.c
@@ -0,0 +1,444 @@
+/*
+ * Copyright (C) 2013 Chris
+ *
+ * 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 modules/max7456/max7456.c
+ * Maxim MAX7456 single-channel monochrome on-screen display driver.
+ *
+ */
+
+#include "std.h"
+#include "stdio.h"
+
+#include "mcu_periph/sys_time.h"
+#include "mcu_periph/gpio.h"
+#include "mcu_periph/spi.h"
+#include "led.h"
+#include "autopilot.h"
+
+#include "subsystems/nav.h"
+#include "generated/flight_plan.h"
+#include "generated/airframe.h"
+#include "subsystems/datalink/datalink.h"
+#include "subsystems/electrical.h"
+
+#include "messages.h"
+#include "subsystems/datalink/downlink.h"
+#include "state.h"
+
+// Peripherials
+#include "max7456.h"
+#include "max7456_regs.h"
+
+#define OSD_STRING_SIZE 31
+#define osd_sprintf _osd_sprintf
+
+static char ascii_to_osd_c( char c);
+static void osd_put_s(char *string, uint8_t attributes, uint8_t char_nb, uint8_t row, uint8_t column);
+static bool_t _osd_sprintf(char* buffer, char* string, float value);
+
+struct spi_transaction max7456_trans;
+
+uint8_t osd_spi_tx_buffer[2];
+uint8_t osd_spi_rx_buffer[2];
+char osd_string[OSD_STRING_SIZE];
+char osd_str_buf[OSD_STRING_SIZE];
+char osd_char = ' ';
+uint8_t step = 0;
+uint16_t osd_char_address = 0;
+uint8_t osd_attr = FALSE;
+
+enum max7456_osd_status_codes {
+ OSD_UNINIT,
+ OSD_INIT1,
+ OSD_INIT2,
+ OSD_INIT3,
+ OSD_INIT4,
+ OSD_READ_STATUS,
+ OSD_IDLE,
+ OSD_S_STEP1,
+ OSD_S_STEP2,
+ OSD_S_STEP3,
+ OSD_FINISHED,
+};
+
+enum osd_attributes{
+ BLINK = OSD_BLINK_CHAR,
+ INVERT = OSD_INVERT_PIXELS,
+};
+
+uint8_t max7456_osd_status = OSD_UNINIT;
+uint8_t osd_enable = TRUE;
+uint8_t osd_enable_val = OSD_IMAGE_ENABLE;
+uint8_t osd_stat_reg = 0;
+bool_t osd_stat_reg_valid = FALSE;
+
+void max7456_init(void) {
+
+ max7456_trans.slave_idx = MAX7456_SLAVE_IDX;
+ max7456_trans.select = SPISelectUnselect;
+ max7456_trans.cpol = SPICpolIdleLow;
+ max7456_trans.cpha = SPICphaEdge1;
+ max7456_trans.dss = SPIDss8bit;
+ max7456_trans.bitorder = SPIMSBFirst;
+ max7456_trans.cdiv = SPIDiv64;
+ max7456_trans.output_length = sizeof(osd_spi_tx_buffer);
+ max7456_trans.output_buf = (uint8_t*) osd_spi_tx_buffer;
+ max7456_trans.input_length = 0;
+ max7456_trans.input_buf = (uint8_t*)osd_spi_rx_buffer;
+ max7456_trans.before_cb = NULL;
+ max7456_trans.after_cb = NULL;
+
+ osd_enable = 1;
+ osd_enable_val = OSD_IMAGE_ENABLE;
+ max7456_osd_status = OSD_UNINIT;
+
+ return;
+}
+
+void max7456_periodic(void) {
+
+ float temp = 0;
+//This code is executed always and checks if the "osd_enable" var has been changed by telemetry.
+//If yes then it commands a reset but this time turns on or off the osd overlay, not the video.
+ if (max7456_osd_status == OSD_IDLE) {
+ if(osd_enable > 1)
+ osd_enable = 1;
+ if ((osd_enable<<3) != osd_enable_val) {
+ osd_enable_val = (osd_enable<<3);
+ max7456_osd_status = OSD_UNINIT;
+ }
+ }
+
+ //INITIALIZATION OF THE OSD
+ if (max7456_osd_status == OSD_UNINIT) {
+ step = 0;
+ max7456_trans.status = SPITransDone;
+ max7456_trans.output_buf[0] = OSD_VM0_REG;
+ //This operation needs at least 100us but when the periodic function will be invoked again
+ //sufficient time will have elapsed even with at a periodic frequency of 1000 Hz
+ max7456_trans.output_buf[1] = OSD_RESET;
+ max7456_osd_status = OSD_INIT1;
+ spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
+ }
+ else
+ if (max7456_osd_status == OSD_INIT2) {
+ max7456_trans.output_length = 1;
+ max7456_trans.input_length = 1;
+ max7456_trans.output_buf[0] = OSD_OSDBL_REG_R;
+ max7456_osd_status = OSD_INIT3;
+ spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
+ }
+ else
+ if (max7456_osd_status == OSD_IDLE && osd_enable > 0) { // DRAW THE OSD SCREEN
+ //draw_osd();
+ switch (step) {
+ case (0):
+ osd_put_s("HDG", FALSE, 3, 0, 13);
+ step = 10;
+ break;
+ case (10):
+ temp = ((float)electrical.vsupply)/10;
+ osd_sprintf(osd_string, "%.1fV", temp );
+ if (temp > LOW_BAT_LEVEL)
+ osd_put_s(osd_string, FALSE, 8, 0, 2);
+ else
+ osd_put_s(osd_string, BLINK|INVERT, 8, 0, 2);
+ step = 20;
+ break;
+ case (20):
+ #if MAG_HEADING_AVAILABLE && !defined(SITL)
+ temp = DegOfRad(MAG_Heading);
+ if (temp < 0)
+ temp += 360;
+ #else
+ temp = DegOfRad(state.h_speed_dir_f);
+ if (temp < 0)
+ temp += 360;
+ #endif
+ osd_sprintf(osd_string, "%.0f", temp);
+ osd_put_s(osd_string, FALSE, 8, 1, 13);
+ step = 30;
+ break;
+ case (30):
+ osd_sprintf(osd_string, "%.0fKm", (state.h_speed_norm_f*3.6));
+ osd_put_s(osd_string, FALSE, 8, 0, 24);
+ step = 40;
+ break;
+ case (40):
+ osd_sprintf(osd_string, "%.0fm", GetPosAlt() );
+ osd_put_s(osd_string, FALSE, 10, 13, 2);
+ step = 50;
+ break;
+ case (50):
+ osd_sprintf(osd_string, "%.1fVZ", stateGetSpeedEnu_f()->z);
+ osd_put_s(osd_string, FALSE, 7, 13, 24);
+ step = 10;
+ break;
+ default: break;
+ }
+ }
+ return;
+}
+
+void max7456_event(void) {
+
+ static uint8_t x = 0;
+
+ if (max7456_trans.status == SPITransSuccess) {
+ max7456_trans.status = SPITransDone;
+
+ switch (max7456_osd_status) {
+ case (OSD_INIT1):
+ max7456_osd_status = OSD_INIT2;
+ break;
+ case (OSD_INIT3):
+ max7456_trans.output_length = 2;
+ max7456_trans.input_length = 0;
+ max7456_trans.output_buf[0] = OSD_OSDBL_REG;
+ max7456_trans.output_buf[1] = max7456_trans.input_buf[0] & (~(1<<4));
+ max7456_osd_status = OSD_INIT4;
+ spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
+ break;
+ case (OSD_INIT4):
+ max7456_trans.output_buf[0] = OSD_VM0_REG;
+#if USE_PAL_FOR_OSD_VIDEO
+ max7456_trans.output_buf[1] = OSD_VIDEO_MODE_PAL|osd_enable_val;
+#else
+ max7456_trans.output_buf[1] = osd_enable_val;
+#endif
+ max7456_osd_status = OSD_FINISHED;
+ spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
+ break;
+ case (OSD_READ_STATUS):
+ osd_stat_reg = max7456_trans.input_buf[0];
+ osd_stat_reg_valid = TRUE;
+ max7456_osd_status = OSD_FINISHED;
+ break;
+ case (OSD_S_STEP1):
+ max7456_trans.output_length = 2;
+ max7456_trans.output_buf[0] = OSD_DMAL_REG;
+ max7456_trans.output_buf[1] = (uint8_t)(osd_char_address);
+ max7456_osd_status = OSD_S_STEP2;
+ spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
+ break;
+ case (OSD_S_STEP2):
+ max7456_trans.output_length = 2;
+ max7456_trans.output_buf[0] = OSD_DMM_REG;
+ max7456_trans.output_buf[1] = OSD_AUTO_INCREMENT_MODE | osd_attr;
+ max7456_osd_status = OSD_S_STEP3;
+ spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
+ x = 0;
+ break;
+ case (OSD_S_STEP3):
+ max7456_trans.output_length = 1; //1 byte tranfers, auto address incrementing.
+ if (osd_string[x] != 0XFF) {
+ max7456_trans.output_buf[0] = osd_string[x++];
+ spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
+ }
+ else {
+ max7456_trans.output_buf[0] = 0xFF; //Exit the auto increment mode
+ max7456_osd_status = OSD_FINISHED;
+ spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
+ }
+ break;
+ case (OSD_FINISHED):
+ osd_attr = 0;
+ max7456_trans.status = SPITransDone;
+ max7456_osd_status = OSD_IDLE;
+ break;
+ default: break;
+ }
+ }
+ return;
+}
+
+static char ascii_to_osd_c(char c) {
+
+ if (c >= '0' && c <= '9') {
+ if (c == '0')
+ c -= 38;
+ else
+ c -= 48;
+ }
+ else {
+ if (c >= 'A' && c <= 'Z')
+ c -= 54;
+ else {
+ if (c >= 'a' && c <= 'z')
+ c -= 60;
+ else {
+ switch (c) {
+ case('('): c = 0x3f; break;
+ case(')'): c = 0x40; break;
+ case('.'): c = 0x41; break;
+ case('?'): c = 0x42; break;
+ case(';'): c = 0x43; break;
+ case(':'): c = 0x44; break;
+ case(','): c = 0x45; break;
+ //case('''): c = 0x46; break;
+ case('/'): c = 0x47; break;
+ case('"'): c = 0x48; break;
+ case('-'): c = 0x49; break;
+ case('<'): c = 0x4A; break;
+ case('>'): c = 0x4B; break;
+ case('@'): c = 0x4C; break;
+ case(' '): c = 0x00; break;
+ case('\0'): c = 0xFF; break;
+ default : break;
+ }
+ }
+ }
+ }
+ return(c);
+}
+
+static void osd_put_s(char *string, uint8_t attributes, uint8_t char_nb, uint8_t row, uint8_t column){
+
+ uint8_t x=0;
+
+ if (row > 15)
+ column = 15;
+ if (column > 29)
+ column = 29;
+ osd_char_address = ((uint16_t)row*30) + column;
+
+ // translate the string and put it to the "osd_string" '\0' = 0xff
+ x = 0;
+ while (*(string+x) != '\0') {
+ osd_string[x] = ascii_to_osd_c(*(string+x));
+ x++;
+ }
+ osd_string[x] = ascii_to_osd_c(*(string+x));
+
+ for (x=0; x < sizeof(osd_string); x++) {
+ if(osd_string[x] == 0xff)
+ break;
+ }
+
+ //Adjust for the reserved character number.
+ for (; x< char_nb; x++) {
+ osd_string[x] = 0;
+ }
+ osd_string[x] = 0xff;
+
+ osd_attr = attributes;
+
+ //TRIGGER THE SPI TRANSFERS. The rest of the spi transfers occur in the "max7456_event" function.
+ if (max7456_osd_status == OSD_IDLE){
+ max7456_trans.output_length = 2;
+ max7456_trans.output_buf[0] = OSD_DMAH_REG;
+ max7456_trans.output_buf[1] = (uint8_t)((osd_char_address>>8) & 0x0001);
+ max7456_osd_status = OSD_S_STEP1;
+ spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
+ }
+ return;
+}
+
+// A VERY VERY STRIPED DOWN sprintf function suitable only for the paparazzi OSD.
+static bool_t _osd_sprintf(char* buffer, char* string, float value) {
+
+ uint8_t param_start = 0;
+ uint8_t param_end = 0;
+ uint8_t frac_nb = 0;
+ uint8_t digit = 0;
+ uint8_t x = 0, y = 0, z = 0;
+
+ uint16_t i_dec = 0;
+ uint16_t i_frac = 0;
+
+ char to_asc[10] = {48,48,48,48,48,48,48,48,48,48};
+
+ // Clear the osd string.
+ for (x=0; x < sizeof(osd_string); x++) {
+ osd_string[x] = 0;
+ }
+ x = 0;
+ // Search for the prameter start and stop positions.
+ while (*(string+x) != '\0'){
+ if (*(string+x) == '%'){
+ param_start = x;
+ }
+ else
+ if (*(string+x) == 'f') {
+ param_end = x;
+ break;
+ }
+ x++;
+ }
+ // find and bound the precision specified.
+ frac_nb = *(string+param_end-1) - 48; // Convert to number, ASCII 48 = '0'
+ if(frac_nb > 3) {
+ frac_nb = 3; // Bound value.
+ }
+ y = (sizeof(to_asc)- 1); // Point y to the end of the array.
+ i_dec = abs((int16_t)value);
+ // Fist we will deal with the fractional part if specified.
+ if (frac_nb > 0 && frac_nb <= 3) {
+ i_frac = abs((int16_t)((value - (int16_t)value) * 1000)); // Max precision is 3 digits.
+ x = 100;
+ z = frac_nb;
+ do { // Example if frac_nb=2 then 952 will show as .95
+ z--;
+ digit = (i_frac / x);
+ to_asc[y+z] = digit + 48; // Convert to ASCII
+ i_frac -= digit * x; // Calculate the remainder.
+ x /= 10; // 952-(9*100) = 52, 52-(10*5)=2 etc.
+ } while(z > 0);
+ y -= frac_nb; // set y to point where the dot must be placed.
+ to_asc[y] = '.';
+ y--; // Set y to point where the rest of the numbers must be written.
+ }
+
+ // Now it is time for the integer part. "y" already points to the position just before the dot.
+ do {
+ to_asc[y] = (i_dec % 10) + 48; //Write at least one digit even if value is zero.
+ i_dec /= 10;
+ if (i_dec <= 0) { // This way the leading zero is ommited.
+ if(value < 0) {
+ y--; to_asc[y] = '-'; // Place the minus sign if needed.
+ }
+ break;
+ }
+ else
+ y--;
+ } while(1);
+
+ // Fill the buffer with the characters in the beggining of the string if any.
+ for (x=0; x
+ * Copyright (C) 2013 Chris
*
* This file is part of paparazzi.
*
@@ -17,19 +17,19 @@
* 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 AHRS_FLOAT_EKF_H
-#define AHRS_FLOAT_EKF_H
+#ifndef MAX7456_H
+#define MAX7456_H
+
+#include "std.h"
+
+extern void max7456_init(void);
+extern void max7456_periodic(void);
+extern void max7456_event(void);
+
+extern uint8_t osd_enable;
-
-extern void ahrs_init(void);
-extern void ahrs_align(void);
-extern void ahrs_propagate(void);
-extern void ahrs_update(void);
-
-
-
-
-#endif /* AHRS_FLOAT_EKF_H */
+#endif //MAX7456_H
diff --git a/sw/airborne/modules/max7456/max7456_regs.h b/sw/airborne/modules/max7456/max7456_regs.h
new file mode 100755
index 0000000000..0b81763074
--- /dev/null
+++ b/sw/airborne/modules/max7456/max7456_regs.h
@@ -0,0 +1,107 @@
+#ifndef MAX7456_REGS_H
+#define MAX7456_REGS_H
+
+//OSD REGISTER ADDRESSES
+#define OSD_VM0_REG 0x00
+#define OSD_VM1_REG 0x01
+#define OSD_DMM_REG 0x04
+#define OSD_DMAH_REG 0x05
+#define OSD_DMAL_REG 0x06
+#define OSD_DMDI_REG 0x07
+#define OSD_OSDBL_REG 0x6C
+#define OSD_OSDBL_REG_R 0xEC
+#define OSD_STAT_REG 0xA0
+
+//OSD BIT POSITIONS
+#define OSD_VIDEO_MODE_PAL (1<<6) // Default = NTSC
+#define OSD_SYNC_INTERNAL ((1<<5)|(1<<4)) // Default = AUTO
+#define OSD_SYNC_EXTERNAL ((1<<5) // Default = AUTO
+#define OSD_IMAGE_ENABLE (1<<3) // Default = OSD OFF
+#define OSD_REFRESH_ON_NEXT_VSYNC (1<<2) // Default = immediately refresh video
+#define OSD_RESET (1<<1) // VM0 reg, hardware set to 0 after reset
+#define OSD_VOUT_DISABLE (1<<0) // default= VIDEO OUT ENABLED
+#define OSD_8BIT_MODE (1<<6) // default= 16 BIT MODE
+#define OSD_BLINK_CHAR (1<<4) // default= No BLINKING
+#define OSD_INVERT_PIXELS (1<<3) // default= No INVERSION
+#define OSD_CLEAR_DISPLAY_MEMORY (1<<2) // DMM reg, default = 0
+#define OSD_AUTO_INCREMENT_MODE (1<<0) // default = NO AUTO INCREMENT
+
+// MAX7456 VIDEO_MODE_0 register
+#define VIDEO_MODE_0_WRITE 0x00
+#define VIDEO_MODE_0_READ 0x80
+#define VIDEO_MODE_0_40_PAL 0x40
+#define VIDEO_MODE_0_20_NoAutoSync 0x20
+#define VIDEO_MODE_0_10_SyncInt 0x10
+#define VIDEO_MODE_0_08_EnOSD 0x08
+#define VIDEO_MODE_0_04_UpdateVsync 0x04
+#define VIDEO_MODE_0_02_Reset 0x02
+#define VIDEO_MODE_0_01_EnVideo 0x01
+// VIDEO MODE 0 bitmap
+#define NTSC 0x00
+#define PAL 0x40
+#define AUTO_SYNC 0x00
+#define EXT_SYNC 0x20
+#define INT_SYNC 0x30
+#define OSD_EN 0x08
+#define VERT_SYNC_IMM 0x00
+#define VERT_SYNC_VSYNC 0x04
+#define SW_RESET 0x02
+#define BUF_EN 0x00
+#define BUF_DI 0x01
+
+// MAX7456 VIDEO_MODE_1 register
+#define VIDEO_MODE_1_WRITE 0x01
+#define VIDEO_MODE_1_READ 0x81
+
+// MAX7456 DM_MODE register
+#define DM_MODE_WRITE 0x04
+#define DM_MODE_READ 0x84
+
+// MAX7456 DM_ADDRH register
+#define DM_ADDRH_WRITE 0x05
+#define DM_ADDRH_READ 0x85
+
+// MAX7456 DM_ADDRL register
+#define DM_ADDRL_WRITE 0x06
+#define DM_ADDRL_READ 0x87
+
+// MAX7456 DM_CODE_IN register
+#define DM_CODE_IN_WRITE 0x07
+#define DM_CODE_IN_READ 0x87
+
+// MAX7456 DM_CODE_OUT register
+#define DM_CODE_OUT_READ 0xB0
+
+// MAX7456 FM_MODE register
+#define FM_MODE_WRITE 0x08
+#define FM_MODE_READ 0x88
+
+// MAX7456 FM_ADDRH register
+#define FM_ADDRH_WRITE 0x09
+#define FM_ADDRH_READ 0x89
+
+// MAX7456 FM_ADDRL register
+#define FM_ADDRL_WRITE 0x0A
+#define FM_ADDRL_READ 0x8A
+
+// MAX7456 FM_DATA_IN register
+#define FM_DATA_IN_WRITE 0x0B
+#define FM_DATA_IN_READ 0x8B
+
+// MAX7456 FM_DATA_OUT register
+#define FM_DATA_OUT_READ 0xC0
+
+// MAX7456 STATUS register
+#define STATUS_READ 0xA0
+#define STATUS_40_RESET_BUSY 0x40
+#define STATUS_20_NVRAM_BUSY 0x20
+#define STATUS_04_LOSS_OF_SYNC 0x04
+#define STATUS_02_PAL_DETECTED 0x02
+#define STATUS_01_NTSC_DETECTED 0x01
+
+// MAX7456 requires clearing OSD Black Level register bit 0x10 after reset
+#define OSDBL_WR 0x6C
+#define OSDBL_RD 0xEC
+#define OSDBL_10_DisableAutoBlackLevel 0x10
+
+#endif //MAX7456_REGS_H
diff --git a/sw/airborne/modules/nav/zamboni_survey.c b/sw/airborne/modules/nav/zamboni_survey.c
new file mode 100644
index 0000000000..4d45187bcd
--- /dev/null
+++ b/sw/airborne/modules/nav/zamboni_survey.c
@@ -0,0 +1,212 @@
+/*
+ * Copyright (C) 2013 Jorn Anke, Felix Ruess
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file modules/nav/zamboni_survey.c
+ *
+ * Zamboni pattern survey for fixedwings.
+ */
+
+#include "modules/nav/zamboni_survey.h"
+
+#include "subsystems/nav.h"
+#include "autopilot.h"
+#include "generated/flight_plan.h"
+
+#ifdef DIGITAL_CAM
+#include "modules/digital_cam/dc.h"
+#endif
+
+
+struct ZamboniSurvey zs;
+
+/**
+ * initializes the variables needed for the survey to start.
+ *
+ * @param center_wp the waypoint defining the center of the survey-rectangle
+ * @param dir_wp the waypoint defining the orientation of the survey-rectangle
+ * @param sweep_length the length of the survey-rectangle
+ * @param sweep_spacing distance between the sweeps
+ * @param sweep_lines number of sweep_lines to fly
+ * @param altitude the altitude that must be reached before the flyover starts
+ */
+bool_t init_zamboni_survey(uint8_t center_wp, uint8_t dir_wp, float sweep_length, float sweep_spacing, int sweep_lines, float altitude)
+{
+ zs.current_laps = 0;
+ zs.pre_leave_angle = 2;
+
+ // copy variables from the flight plan
+ VECT2_COPY(zs.wp_center, waypoints[center_wp]);
+ VECT2_COPY(zs.wp_dir, waypoints[dir_wp]);
+ zs.altitude = altitude;
+
+ // if turning right leave circle before angle is reached, if turning left - leave after
+ if (sweep_spacing > 0) {
+ zs.pre_leave_angle -= zs.pre_leave_angle;
+ }
+
+ struct FloatVect2 flight_vec;
+ VECT2_DIFF(flight_vec, zs.wp_dir, zs.wp_center);
+ FLOAT_VECT2_NORMALIZE(flight_vec);
+
+ // calculate the flight_angle
+ zs.flight_angle = DegOfRad(atan2(flight_vec.x, flight_vec.y));
+ zs.return_angle = zs.flight_angle + 180;
+ if (zs.return_angle > 359) {
+ zs.return_angle -= 360;
+ }
+
+ // calculate the vector from one flightline perpendicular to the next flightline left,
+ // seen in the flightdirection. (Delta x and delta y betwen two adjecent flightlines)
+ // (used later to move the flight pattern one flightline up for each round)
+ zs.sweep_width.x = -flight_vec.y * sweep_spacing;
+ zs.sweep_width.y = +flight_vec.x * sweep_spacing;
+
+ // calculate number of laps to fly and turning radius for each end
+ zs.total_laps = (sweep_lines+1)/2;
+ zs.turnradius1 = (zs.total_laps-1) * sweep_spacing * 0.5;
+ zs.turnradius2 = zs.total_laps * sweep_spacing * 0.5;
+
+ struct FloatVect2 flight_line;
+ VECT2_SMUL(flight_line, flight_vec, sweep_length * 0.5);
+
+ //CALCULATE THE NAVIGATION POINTS
+ //start and end of flight-line in flight-direction
+ VECT2_DIFF(zs.seg_start, zs.wp_center, flight_line);
+ VECT2_SUM(zs.seg_end, zs.wp_center, flight_line);
+
+ struct FloatVect2 sweep_span;
+ VECT2_SMUL(sweep_span, zs.sweep_width, zs.total_laps-1);
+ //start and end of flight-line in return-direction
+ VECT2_DIFF(zs.ret_start, zs.seg_end, sweep_span);
+ VECT2_DIFF(zs.ret_end, zs.seg_start, sweep_span);
+
+ //turn-centers at both ends
+ zs.turn_center1.x = (zs.seg_end.x + zs.ret_start.x) / 2.0;
+ zs.turn_center1.y = (zs.seg_end.y + zs.ret_start.y) / 2.0;
+ zs.turn_center2.x = (zs.seg_start.x + zs.ret_end.x + zs.sweep_width.x) / 2.0;
+ zs.turn_center2.y = (zs.seg_start.y + zs.ret_end.y + zs.sweep_width.y) / 2.0;
+
+ //fast climbing to desired altitude
+ NavVerticalAutoThrottleMode(100.0);
+ NavVerticalAltitudeMode(zs.altitude, 0.0);
+
+ zs.stage = Z_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 zamboni_survey(void)
+{
+ // retain altitude
+ NavVerticalAutoThrottleMode(0.0);
+ NavVerticalAltitudeMode(zs.altitude, 0.0);
+
+ //go from center of field to end of field - (before starting the syrvey)
+ if (zs.stage == Z_ENTRY) {
+ nav_route_xy(zs.wp_center.x, zs.wp_center.y, zs.seg_end.x, zs.seg_end.y);
+ if (nav_approaching_xy(zs.seg_end.x, zs.seg_end.y, zs.wp_center.x, zs.wp_center.y, CARROT)) {
+ zs.stage = Z_TURN1;
+ NavVerticalAutoThrottleMode(0.0);
+ nav_init_stage();
+ }
+ }
+
+ //Turn from stage to return
+ else if (zs.stage == Z_TURN1) {
+ nav_circle_XY(zs.turn_center1.x, zs.turn_center1.y, zs.turnradius1);
+ if (NavCourseCloseTo(zs.return_angle + zs.pre_leave_angle)){
+ // && nav_approaching_xy(zs.seg_end.x, zs.seg_end.y, zs.seg_start.x, zs.seg_start.y, CARROT
+ //calculate SEG-points for the next flyover
+ VECT2_ADD(zs.seg_start, zs.sweep_width);
+ VECT2_ADD(zs.seg_end, zs.sweep_width);
+
+ zs.stage = Z_RET;
+ nav_init_stage();
+#ifdef DIGITAL_CAM
+ LINE_START_FUNCTION;
+#endif
+ }
+ }
+
+ //fly the segment until seg_end is reached
+ else if (zs.stage == Z_RET) {
+ nav_route_xy(zs.ret_start.x, zs.ret_start.y, zs.ret_end.x, zs.ret_end.y);
+ if (nav_approaching_xy(zs.ret_end.x, zs.ret_end.y, zs.ret_start.x, zs.ret_start.y, 0)) {
+ zs.current_laps = zs.current_laps + 1;
+#ifdef DIGITAL_CAM
+ //dc_stop();
+ LINE_STOP_FUNCTION;
+#endif
+ zs.stage = Z_TURN2;
+ }
+ }
+
+ //turn from stage to return
+ else if (zs.stage == Z_TURN2) {
+ nav_circle_XY(zs.turn_center2.x, zs.turn_center2.y, zs.turnradius2);
+ if (NavCourseCloseTo(zs.flight_angle + zs.pre_leave_angle)) {
+ //zs.current_laps = zs.current_laps + 1;
+ zs.stage = Z_SEG;
+ nav_init_stage();
+#ifdef DIGITAL_CAM
+ LINE_START_FUNCTION;
+#endif
+ }
+
+ //return
+ } else if (zs.stage == Z_SEG) {
+ nav_route_xy(zs.seg_start.x, zs.seg_start.y, zs.seg_end.x, zs.seg_end.y);
+ if (nav_approaching_xy(zs.seg_end.x, zs.seg_end.y, zs.seg_start.x, zs.seg_start.y, 0)) {
+
+ // calculate the rest of the points for the next fly-over
+ VECT2_ADD(zs.ret_start, zs.sweep_width);
+ VECT2_ADD(zs.ret_end, zs.sweep_width);
+ zs.turn_center1.x = (zs.seg_end.x + zs.ret_start.x)/2;
+ zs.turn_center1.y = (zs.seg_end.y + zs.ret_start.y)/2;
+ zs.turn_center2.x = (zs.seg_start.x + zs.ret_end.x + zs.sweep_width.x) / 2;
+ zs.turn_center2.y = (zs.seg_start.y + zs.ret_end.y + zs.sweep_width.y) / 2;
+
+ zs.stage = Z_TURN1;
+ nav_init_stage();
+#ifdef DIGITAL_CAM
+ //dc_stop();
+ LINE_STOP_FUNCTION;
+#endif
+ }
+ }
+ if (zs.current_laps >= zs.total_laps) {
+#ifdef DIGITAL_CAM
+ LINE_STOP_FUNCTION;
+#endif
+ return FALSE;
+ }
+ else {
+ return TRUE;
+ }
+}
diff --git a/sw/airborne/modules/nav/zamboni_survey.h b/sw/airborne/modules/nav/zamboni_survey.h
new file mode 100644
index 0000000000..6bfe644ce7
--- /dev/null
+++ b/sw/airborne/modules/nav/zamboni_survey.h
@@ -0,0 +1,76 @@
+/*
+ * Copyright (C) 2013 Jorn Anke, Felix Ruess
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file modules/nav/zamboni_survey.h
+ *
+ * Zamboni pattern survey for fixedwings.
+ */
+
+#ifndef ZAMBONI_SURVEY_H
+#define ZAMBONI_SURVEY_H
+
+#include "std.h"
+#include "math/pprz_algebra_float.h"
+
+typedef enum {Z_ERR, Z_ENTRY, Z_SEG, Z_TURN1, Z_RET, Z_TURN2} z_survey_stage;
+
+struct ZamboniSurvey {
+ /* variables used to store values from the flight plan */
+ struct FloatVect2 wp_center;
+ struct FloatVect2 wp_dir;
+ struct FloatVect2 sweep_width;
+ float altitude;
+
+ /** in degrees. Leave turncircles a small angle before the 180deg turns are completed
+ * to get a smoother transition to flight-lines
+ */
+ int pre_leave_angle;
+ float flight_angle; ///< in degrees
+ float return_angle; ///< in degrees
+ int current_laps;
+ int total_laps;
+ float turnradius1;
+ float turnradius2;
+ struct FloatVect2 turn_center1;
+ struct FloatVect2 turn_center2;
+ struct FloatVect2 seg_start;
+ struct FloatVect2 seg_end;
+ struct FloatVect2 ret_start;
+ struct FloatVect2 ret_end;
+ /**
+ * z_stage starts at ENTRY and than circles trought the other
+ * states until to rectangle 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
+ */
+ z_survey_stage stage;
+};
+
+
+extern bool_t init_zamboni_survey(uint8_t center_wp, uint8_t dir_wp, float sweep_length, float sweep_spacing, int sweep_lines, float altitude);
+extern bool_t zamboni_survey(void);
+
+#endif //ZAMBONI_SURVEY_H
diff --git a/sw/airborne/modules/sensors/airspeed_adc.c b/sw/airborne/modules/sensors/airspeed_adc.c
index f461d310bf..af9955437e 100644
--- a/sw/airborne/modules/sensors/airspeed_adc.c
+++ b/sw/airborne/modules/sensors/airspeed_adc.c
@@ -60,7 +60,7 @@ void airspeed_adc_update( void ) {
float airspeed = AIRSPEED_SCALE * (adc_airspeed_val - AIRSPEED_BIAS);
#endif
stateSetAirspeed_f(&airspeed);
-#else // SITL
+#elif !defined USE_NPS
extern float sim_air_speed;
stateSetAirspeed_f(&sim_air_speed);
adc_airspeed_val = 0;
diff --git a/sw/airborne/modules/sensors/airspeed_amsys.c b/sw/airborne/modules/sensors/airspeed_amsys.c
index 9ee17dc80f..f2e4ef2e7f 100644
--- a/sw/airborne/modules/sensors/airspeed_amsys.c
+++ b/sw/airborne/modules/sensors/airspeed_amsys.c
@@ -31,12 +31,6 @@
#include
//#include
-#if !USE_AIRSPEED
-// Just a Warning --> We do't use it.
-//#ifndef SENSOR_SYNC_SEND
-//#warning either set USE_AIRSPEED or SENSOR_SYNC_SEND to use amsys_airspeed
-//#endif
-#endif
#define AIRSPEED_AMSYS_ADDR 0xE8 // original F0
#ifndef AIRSPEED_AMSYS_SCALE
@@ -84,108 +78,134 @@ bool_t airspeed_amsys_offset_init;
double airspeed_amsys_offset_tmp;
uint16_t airspeed_amsys_cnt;
+void airspeed_amsys_downlink(void);
+
void airspeed_amsys_init( void ) {
- airspeed_amsys_raw = 0;
- airspeed_amsys = 0.0;
- airspeed_amsys_p = 0.0;
- airspeed_amsys_offset = 0;
- airspeed_amsys_offset_tmp = 0;
- airspeed_amsys_i2c_done = TRUE;
- airspeed_amsys_valid = TRUE;
- airspeed_amsys_offset_init = FALSE;
- airspeed_scale = AIRSPEED_AMSYS_SCALE;
- airspeed_filter = AIRSPEED_AMSYS_FILTER;
- airspeed_amsys_i2c_trans.status = I2CTransDone;
- airspeed_amsys_cnt = AIRSPEED_AMSYS_OFFSET_NBSAMPLES_INIT + AIRSPEED_AMSYS_OFFSET_NBSAMPLES_AVRG;
+ airspeed_amsys_raw = 0;
+ airspeed_amsys = 0.0;
+ airspeed_amsys_p = 0.0;
+ airspeed_amsys_offset = 0;
+ airspeed_amsys_offset_tmp = 0;
+ airspeed_amsys_i2c_done = TRUE;
+ airspeed_amsys_valid = TRUE;
+ airspeed_amsys_offset_init = FALSE;
+ airspeed_scale = AIRSPEED_AMSYS_SCALE;
+ airspeed_filter = AIRSPEED_AMSYS_FILTER;
+ airspeed_amsys_i2c_trans.status = I2CTransDone;
+ airspeed_amsys_cnt = AIRSPEED_AMSYS_OFFSET_NBSAMPLES_INIT +
+ AIRSPEED_AMSYS_OFFSET_NBSAMPLES_AVRG;
}
void airspeed_amsys_read_periodic( void ) {
#ifndef SITL
- if (airspeed_amsys_i2c_trans.status == I2CTransDone)
+ if (airspeed_amsys_i2c_trans.status == I2CTransDone) {
#ifndef MEASURE_AMSYS_TEMPERATURE
- i2c_receive(&AIRSPEED_AMSYS_I2C_DEV, &airspeed_amsys_i2c_trans, AIRSPEED_AMSYS_ADDR, 2);
+ i2c_receive(&AIRSPEED_AMSYS_I2C_DEV, &airspeed_amsys_i2c_trans, AIRSPEED_AMSYS_ADDR, 2);
#else
- i2c_receive(&AIRSPEED_AMSYS_I2C_DEV, &airspeed_amsys_i2c_trans, AIRSPEED_AMSYS_ADDR, 4);
+ i2c_receive(&AIRSPEED_AMSYS_I2C_DEV, &airspeed_amsys_i2c_trans, AIRSPEED_AMSYS_ADDR, 4);
#endif
+ }
#if USE_AIRSPEED
stateSetAirspeed_f(&airspeed_amsys);
#endif
-#else // SITL
+#elif !defined USE_NPS
extern float sim_air_speed;
stateSetAirspeed_f(&sim_air_speed);
#endif //SITL
-#ifdef AIRSPEED_AMSYS_SYNC_SEND
- DOWNLINK_SEND_AMSYS_AIRSPEED(DefaultChannel, DefaultDevice, &airspeed_amsys_raw, &airspeed_amsys_p, &airspeed_amsys_tmp, &airspeed_amsys, &airspeed_amsys_offset);
-#else
- RunOnceEvery(10, DOWNLINK_SEND_AMSYS_AIRSPEED(DefaultChannel, DefaultDevice, &airspeed_amsys_raw, &airspeed_amsys_p, &airspeed_amsys_tmp, &airspeed_amsys, &airspeed_temperature));
+#ifndef AIRSPEED_AMSYS_SYNC_SEND
+ RunOnceEvery(10, airspeed_amsys_downlink());
#endif
}
+void airspeed_amsys_downlink(void) {
+ DOWNLINK_SEND_AMSYS_AIRSPEED(DefaultChannel, DefaultDevice,
+ &airspeed_amsys_raw, &airspeed_amsys_p,
+ &airspeed_amsys_tmp, &airspeed_amsys,
+ &airspeed_temperature);
+}
+
void airspeed_amsys_read_event( void ) {
- // Get raw airspeed from buffer
- airspeed_amsys_raw = 0;
- airspeed_amsys_raw = (airspeed_amsys_i2c_trans.buf[0]<<8) | airspeed_amsys_i2c_trans.buf[1];
+ // Get raw airspeed from buffer
+ airspeed_amsys_raw = 0;
+ airspeed_amsys_raw = (airspeed_amsys_i2c_trans.buf[0]<<8) | airspeed_amsys_i2c_trans.buf[1];
#ifdef MEASURE_AMSYS_TEMPERATURE
- tempAS_amsys_raw = (airspeed_amsys_i2c_trans.buf[2]<<8) | airspeed_amsys_i2c_trans.buf[3];
- airspeed_temperature = (float)((float)(tempAS_amsys_raw-TEMPERATURE_AMSYS_OFFSET_MIN)/((float)(TEMPERATURE_AMSYS_OFFSET_MAX-TEMPERATURE_AMSYS_OFFSET_MIN)/TEMPERATURE_AMSYS_MAX)+TEMPERATURE_AMSYS_MIN);// Tmin=-25, Tmax=85
+ tempAS_amsys_raw = (airspeed_amsys_i2c_trans.buf[2]<<8) | airspeed_amsys_i2c_trans.buf[3];
+ const float temp_off_scale = (float)(TEMPERATURE_AMSYS_MAX) /
+ (TEMPERATURE_AMSYS_OFFSET_MAX - TEMPERATURE_AMSYS_OFFSET_MIN);
+ // Tmin=-25, Tmax=85
+ airspeed_temperature = temp_off_scale * (tempAS_amsys_raw - TEMPERATURE_AMSYS_OFFSET_MIN) +
+ TEMPERATURE_AMSYS_MIN;
#endif
- // Check if this is valid airspeed
- if (airspeed_amsys_raw == 0)
- airspeed_amsys_valid = FALSE;
- else
- airspeed_amsys_valid = TRUE;
+ // Check if this is valid airspeed
+ if (airspeed_amsys_raw == 0)
+ airspeed_amsys_valid = FALSE;
+ else
+ airspeed_amsys_valid = TRUE;
- // Continue only if a new airspeed value was received
- if (airspeed_amsys_valid) {
+ // Continue only if a new airspeed value was received
+ if (airspeed_amsys_valid) {
- // raw not under offest min
- if (airspeed_amsys_rawAIRSPEED_AMSYS_OFFSET_MAX)
- airspeed_amsys_raw = AIRSPEED_AMSYS_OFFSET_MAX;
+ // raw not under offest min
+ if (airspeed_amsys_raw < AIRSPEED_AMSYS_OFFSET_MIN)
+ airspeed_amsys_raw = AIRSPEED_AMSYS_OFFSET_MIN;
+ // raw not over offest max
+ if (airspeed_amsys_raw > AIRSPEED_AMSYS_OFFSET_MAX)
+ airspeed_amsys_raw = AIRSPEED_AMSYS_OFFSET_MAX;
- // calculate raw to pressure
- airspeed_amsys_p = (float)(airspeed_amsys_raw-AIRSPEED_AMSYS_OFFSET_MIN)*AIRSPEED_AMSYS_MAXPRESURE/(float)(AIRSPEED_AMSYS_OFFSET_MAX-AIRSPEED_AMSYS_OFFSET_MIN);
- if (!airspeed_amsys_offset_init) {
- --airspeed_amsys_cnt;
- // Check if averaging completed
- if (airspeed_amsys_cnt == 0) {
- // Calculate average
- airspeed_amsys_offset = (float)(airspeed_amsys_offset_tmp / AIRSPEED_AMSYS_OFFSET_NBSAMPLES_AVRG);
- airspeed_amsys_offset_init = TRUE;
- }
- // Check if averaging needs to continue
- else if (airspeed_amsys_cnt <= AIRSPEED_AMSYS_OFFSET_NBSAMPLES_AVRG)
- airspeed_amsys_offset_tmp += airspeed_amsys_p;
+ // calculate raw to pressure
+ const float p_off_scale = (float)(AIRSPEED_AMSYS_MAXPRESURE) /
+ (AIRSPEED_AMSYS_OFFSET_MAX - AIRSPEED_AMSYS_OFFSET_MIN);
+ airspeed_amsys_p = p_off_scale * (airspeed_amsys_raw - AIRSPEED_AMSYS_OFFSET_MIN);
- airspeed_amsys = 0.;
+ if (!airspeed_amsys_offset_init) {
+ --airspeed_amsys_cnt;
+ // Check if averaging completed
+ if (airspeed_amsys_cnt == 0) {
+ // Calculate average
+ airspeed_amsys_offset = airspeed_amsys_offset_tmp / AIRSPEED_AMSYS_OFFSET_NBSAMPLES_AVRG;
+ airspeed_amsys_offset_init = TRUE;
+ }
+ // Check if averaging needs to continue
+ else if (airspeed_amsys_cnt <= AIRSPEED_AMSYS_OFFSET_NBSAMPLES_AVRG) {
+ airspeed_amsys_offset_tmp += airspeed_amsys_p;
+ }
- }
- else {
- airspeed_amsys_p = airspeed_amsys_p - airspeed_amsys_offset;
- if (airspeed_amsys_p <= 0) airspeed_amsys_p=0.000000001;
- airspeed_amsys_tmp = sqrtf(2*(airspeed_amsys_p)*airspeed_scale/1.2041); // convert pressure to airspeed
- // Lowpassfiltering
- airspeed_amsys = airspeed_filter * airspeed_old + (1 - airspeed_filter) * airspeed_amsys_tmp;
- airspeed_old = airspeed_amsys;
- //New value available
- }
+ airspeed_amsys = 0.;
- } /*else {
- airspeed_amsys = 0.0;
- }*/
+ }
+ else {
+ airspeed_amsys_p = airspeed_amsys_p - airspeed_amsys_offset;
+ if (airspeed_amsys_p <= 0)
+ airspeed_amsys_p = 0.000000001;
+ // convert pressure to airspeed
+ airspeed_amsys_tmp = sqrtf(2 * airspeed_amsys_p * airspeed_scale / 1.2041);
+ // Lowpassfiltering
+ airspeed_amsys = airspeed_filter * airspeed_old +
+ (1.0 - airspeed_filter) * airspeed_amsys_tmp;
+ airspeed_old = airspeed_amsys;
+
+ //New value available
+#if USE_AIRSPEED
+ stateSetAirspeed_f(&airspeed_amsys);
+#endif
+#ifdef AIRSPEED_AMSYS_SYNC_SEND
+ airspeed_amsys_downlink();
+#endif
+ }
+
+ }
+ /*else {
+ airspeed_amsys = 0.0;
+ }*/
-
-
- // Transaction has been read
- airspeed_amsys_i2c_trans.status = I2CTransDone;
+ // Transaction has been read
+ airspeed_amsys_i2c_trans.status = I2CTransDone;
}
diff --git a/sw/airborne/modules/sensors/airspeed_ets.c b/sw/airborne/modules/sensors/airspeed_ets.c
index 15eff2ce56..f7d2fed2fc 100644
--- a/sw/airborne/modules/sensors/airspeed_ets.c
+++ b/sw/airborne/modules/sensors/airspeed_ets.c
@@ -125,7 +125,7 @@ void airspeed_ets_read_periodic( void ) {
}
if (airspeed_ets_i2c_trans.status == I2CTransDone)
i2c_receive(&AIRSPEED_ETS_I2C_DEV, &airspeed_ets_i2c_trans, AIRSPEED_ETS_ADDR, 2);
-#else // SITL
+#elif !defined USE_NPS
extern float sim_air_speed;
stateSetAirspeed_f(&sim_air_speed);
#endif //SITL
diff --git a/sw/airborne/modules/sensors/AOA_adc.c b/sw/airborne/modules/sensors/aoa_adc.c
similarity index 61%
rename from sw/airborne/modules/sensors/AOA_adc.c
rename to sw/airborne/modules/sensors/aoa_adc.c
index 28dbc0e03b..14af6512b4 100644
--- a/sw/airborne/modules/sensors/AOA_adc.c
+++ b/sw/airborne/modules/sensors/aoa_adc.c
@@ -21,14 +21,14 @@
*/
/**
- * @file modules/sensors/AOA_adc.c
+ * @file modules/sensors/aoa_adc.c
* @brief Angle of Attack sensor on ADC
* Autor: Bruzzlee
*
* ex: US DIGITAL MA3-A10-236-N
*/
-#include "modules/sensors/AOA_adc.h"
+#include "modules/sensors/aoa_adc.h"
#include "generated/airframe.h"
#include "state.h"
@@ -37,31 +37,22 @@
#include "messages.h"
#include "subsystems/datalink/downlink.h"
-// Default offset value (assuming 0 AOA is in the middle of the range)
+/// Default offset value (assuming 0 AOA is in the middle of the range)
#ifndef AOA_OFFSET
#define AOA_OFFSET M_PI
#endif
-// Default filter value
+/// Default filter value
#ifndef AOA_FILTER
#define AOA_FILTER 0.5
#endif
-// Default sensitivity (2*pi on a 10 bit ADC)
+/// Default sensitivity (2*pi on a 10 bit ADC)
#ifndef AOA_SENS
#define AOA_SENS ((2.0*M_PI)/1024)
#endif
-uint16_t adc_AOA_val;
-float AOA_offset, AOA_filter;
-
-// Internal values
-float AOA, AOA_old;
// Downlink
-#ifndef SITL // Use ADC if not in simulation
-
-#include "mcu_periph/adc.h"
-
#ifndef ADC_CHANNEL_AOA
#error "ADC_CHANNEL_AOA needs to be defined to use AOA_adc module"
#endif
@@ -70,32 +61,30 @@ float AOA, AOA_old;
#define ADC_CHANNEL_AOA_NB_SAMPLES DEFAULT_AV_NB_SAMPLE
#endif
-struct adc_buf buf_AOA;
-#endif
+struct Aoa_Adc aoa_adc;
-
-void AOA_adc_init( void ) {
- AOA_offset = AOA_OFFSET;
- AOA_filter = AOA_FILTER;
- AOA_old = 0.0;
- AOA = 0.0;
-#ifndef SITL
- adc_buf_channel(ADC_CHANNEL_AOA, &buf_AOA, ADC_CHANNEL_AOA_NB_SAMPLES);
-#endif
+void aoa_adc_init(void) {
+ aoa_adc.offset = AOA_OFFSET;
+ aoa_adc.filter = AOA_FILTER;
+ aoa_adc.sens = AOA_SENS;
+ aoa_adc.angle = 0.0;
+ adc_buf_channel(ADC_CHANNEL_AOA, &aoa_adc.buf, ADC_CHANNEL_AOA_NB_SAMPLES);
}
-void AOA_adc_update( void ) {
-#ifndef SITL
- adc_AOA_val = buf_AOA.sum / buf_AOA.av_nb_sample;
+void aoa_adc_update(void) {
+ static float prev_aoa = 0.0;
+
+ aoa_adc.raw = aoa_adc.buf.sum / aoa_adc.buf.av_nb_sample;
// PT1 filter and convert to rad
- AOA = AOA_filter * AOA_old + (1 - AOA_filter) * (adc_AOA_val*AOA_SENS - AOA_offset);
- AOA_old = AOA;
-#endif
- RunOnceEvery(30, DOWNLINK_SEND_AOA_adc(DefaultChannel, DefaultDevice, &adc_AOA_val, &AOA));
+ aoa_adc.angle = aoa_adc.filter * prev_aoa +
+ (1.0 - aoa_adc.filter) * (aoa_adc.raw * aoa_adc.sens - aoa_adc.offset);
+ prev_aoa = aoa_adc.angle;
#ifdef USE_AOA
- stateSetAngleOfAttack_f(AOA);
+ stateSetAngleOfAttack_f(&aoa_adc.angle);
#endif
+
+ RunOnceEvery(30, DOWNLINK_SEND_AOA_ADC(DefaultChannel, DefaultDevice, &aoa_adc.raw, &aoa_adc.angle));
}
diff --git a/sw/airborne/modules/sensors/AOA_adc.h b/sw/airborne/modules/sensors/aoa_adc.h
similarity index 64%
rename from sw/airborne/modules/sensors/AOA_adc.h
rename to sw/airborne/modules/sensors/aoa_adc.h
index 8ab1741173..02c429c52d 100644
--- a/sw/airborne/modules/sensors/AOA_adc.h
+++ b/sw/airborne/modules/sensors/aoa_adc.h
@@ -21,7 +21,7 @@
*/
/**
- * @file modules/sensors/AOA_adc.c
+ * @file modules/sensors/aoa_adc.c
* @brief Angle of Attack sensor on ADC
* Autor: Bruzzlee
*
@@ -32,20 +32,27 @@
#define AOA_ADC_H
#include "std.h"
+#include "mcu_periph/adc.h"
-/** Raw ADC value */
-extern uint16_t adc_AOA_val;
-
-/** Angle of Attack offset */
-extern float AOA_offset;
-
-/** Filtering value [0-1[
- * 0: no filtering
- * 1: output is a constant value
+/** Angle of Attack sensor via ADC.
*/
-extern float AOA_filter;
+struct Aoa_Adc {
+ struct adc_buf buf;
+ uint16_t raw; ///< raw ADC value
+ float angle; ///< Angle of attack in radians
+ float offset; ///< Angle of attack offset in radians
+ float sens; ///< sensitiviy, i.e. scale to conver raw to angle
-void AOA_adc_init( void );
-void AOA_adc_update( void );
+ /** Filtering value [0-1]
+ * 0: no filtering
+ * 1: output is a constant value
+ */
+ float filter;
+};
+
+extern struct Aoa_Adc aoa_adc;
+
+void aoa_adc_init(void);
+void aoa_adc_update(void);
#endif /* AOA_ADC_H */
diff --git a/sw/airborne/modules/sensors/baro_MS5534A.h b/sw/airborne/modules/sensors/baro_MS5534A.h
index 94568c892d..7e98fa1c34 100644
--- a/sw/airborne/modules/sensors/baro_MS5534A.h
+++ b/sw/airborne/modules/sensors/baro_MS5534A.h
@@ -54,7 +54,7 @@ void baro_MS5534A_event_task( void );
void baro_MS5534A_event( void );
-#define BaroMS5534AUpdate(_b) { if (baro_MS5534A_available) { _b = baro_MS5534A_pressure; baro_MS5534A_available = FALSE; } }
+#define BaroMS5534AUpdate(_b, _h) { if (baro_MS5534A_available) { _b = baro_MS5534A_pressure; _h(); baro_MS5534A_available = FALSE; } }
#endif // USE_BARO_MS5534A
diff --git a/sw/airborne/modules/sensors/baro_amsys.c b/sw/airborne/modules/sensors/baro_amsys.c
index 8160c89672..60ed9919da 100644
--- a/sw/airborne/modules/sensors/baro_amsys.c
+++ b/sw/airborne/modules/sensors/baro_amsys.c
@@ -31,6 +31,7 @@
#ifdef SITL
#include "subsystems/gps.h"
+#include "subsystems/navigation/common_nav.h"
#endif
//Messages
diff --git a/sw/airborne/modules/sensors/baro_amsys.h b/sw/airborne/modules/sensors/baro_amsys.h
index 94429a402e..a6852dab58 100644
--- a/sw/airborne/modules/sensors/baro_amsys.h
+++ b/sw/airborne/modules/sensors/baro_amsys.h
@@ -28,7 +28,8 @@
#include "std.h"
#include "mcu_periph/i2c.h"
-#define BARO_AMSYS_DT 0.05
+/// new measurement every baro_amsys_read_periodic
+#define BARO_AMSYS_DT BARO_AMSYS_READ_PERIODIC_PERIOD
extern uint16_t baro_amsys_adc;
// extern float baro_amsys_offset;
@@ -47,6 +48,6 @@ extern void baro_amsys_read_event( void );
#define BaroAmsysEvent() { if (baro_amsys_i2c_trans.status == I2CTransSuccess) baro_amsys_read_event(); }
-#define BaroAmsysUpdate(_b) { if (baro_amsys_valid) { _b = baro_amsys_adc; baro_amsys_valid = FALSE; } }
+#define BaroAmsysUpdate(_b, _h) { if (baro_amsys_valid) { _b = baro_amsys_adc; _h(); baro_amsys_valid = FALSE; } }
#endif // BARO_AMSYS_H
diff --git a/sw/airborne/modules/sensors/baro_bmp.c b/sw/airborne/modules/sensors/baro_bmp.c
index cba28c6397..622e8aede9 100644
--- a/sw/airborne/modules/sensors/baro_bmp.c
+++ b/sw/airborne/modules/sensors/baro_bmp.c
@@ -1,5 +1,6 @@
/*
* Copyright (C) 2010 Martin Mueller
+ * Copyright (C) 2013 Felix Ruess
*
* This file is part of paparazzi.
*
@@ -20,14 +21,16 @@
*
*/
-/** \file baro_bmp.c
- * \brief Bosch BMP085 I2C sensor interface
+/**
+ * @file modules/sensors/baro_bmp.c
+ * Bosch BMP085 I2C sensor interface.
*
- * This reads the values for pressure and temperature from the Bosch BMP085 sensor through I2C.
+ * This reads the values for pressure and temperature from the Bosch BMP085 sensor through I2C.
*/
#include "baro_bmp.h"
+#include "peripherals/bmp085_regs.h"
#include "mcu_periph/sys_time.h"
#include "mcu_periph/i2c.h"
@@ -35,12 +38,6 @@
#include "mcu_periph/uart.h"
#include "messages.h"
#include "subsystems/datalink/downlink.h"
-#include "state.h"
-#include "subsystems/nav.h"
-
-#ifdef SITL
-#include "subsystems/gps.h"
-#endif
#if !defined(SENSOR_SYNC_SEND) && !defined(USE_BARO_BMP)
@@ -51,196 +48,56 @@
#define BMP_I2C_DEV i2c0
#endif
-#define BMP085_SLAVE_ADDR 0xEE
-#define BARO_BMP_OFFSET_MAX 30000
-#define BARO_BMP_OFFSET_MIN 10
-#define BARO_BMP_OFFSET_NBSAMPLES_INIT 2
-#define BARO_BMP_OFFSET_NBSAMPLES_AVRG 4
#define BARO_BMP_R 0.5
#define BARO_BMP_SIGMA2 0.1
-struct i2c_transaction bmp_trans;
+
+struct Bmp085 baro_bmp;
bool_t baro_bmp_enabled;
float baro_bmp_r;
float baro_bmp_sigma2;
+int32_t baro_bmp_alt;
-// Global variables
-uint8_t baro_bmp_status;
-bool_t baro_bmp_valid;
-uint32_t baro_bmp_pressure;
-uint16_t baro_bmp_temperature;
-int32_t baro_bmp_altitude, baro_bmp,baro_bmp_temp,baro_bmp_offset;
-double tmp_float;
+void baro_bmp_init(void) {
-int16_t bmp_ac1, bmp_ac2, bmp_ac3;
-uint16_t bmp_ac4, bmp_ac5, bmp_ac6;
-int16_t bmp_b1, bmp_b2;
-int16_t bmp_mb, bmp_mc, bmp_md;
-int32_t bmp_up, bmp_ut;
+ bmp085_init(&baro_bmp, &BMP_I2C_DEV, BMP085_SLAVE_ADDR);
-// Local variables
-bool_t baro_bmp_offset_init;
-int32_t baro_bmp_offset_tmp;
-uint16_t baro_bmp_cnt;
-
-void baro_bmp_init( void ) {
- baro_bmp_status = BARO_BMP_UNINIT;
- baro_bmp_valid = FALSE;
baro_bmp_r = BARO_BMP_R;
baro_bmp_sigma2 = BARO_BMP_SIGMA2;
baro_bmp_enabled = TRUE;
- baro_bmp_offset_init = FALSE;
- baro_bmp_cnt = BARO_BMP_OFFSET_NBSAMPLES_INIT + BARO_BMP_OFFSET_NBSAMPLES_AVRG;
- /* read calibration values */
- bmp_trans.buf[0] = BMP085_EEPROM_AC1;
- i2c_transceive(&BMP_I2C_DEV, &bmp_trans, BMP085_SLAVE_ADDR, 1, 22);
-}
-
-void baro_bmp_periodic( void ) {
-#ifndef SITL
- if (baro_bmp_status == BARO_BMP_IDLE) {
- /* start temp measurement (once) */
- bmp_trans.buf[0] = BMP085_CTRL_REG;
- bmp_trans.buf[1] = BMP085_START_TEMP;
- i2c_transmit(&BMP_I2C_DEV, &bmp_trans, BMP085_SLAVE_ADDR, 2);
- baro_bmp_status = BARO_BMP_START_TEMP;
- }
- else if (baro_bmp_status == BARO_BMP_START_TEMP) {
- /* read temp measurement */
- bmp_trans.buf[0] = BMP085_DAT_MSB;
- i2c_transceive(&BMP_I2C_DEV, &bmp_trans, BMP085_SLAVE_ADDR, 1, 2);
- baro_bmp_status = BARO_BMP_READ_TEMP;
- }
- else if (baro_bmp_status == BARO_BMP_START_PRESS) {
- /* read press measurement */
- bmp_trans.buf[0] = BMP085_DAT_MSB;
- i2c_transceive(&BMP_I2C_DEV, &bmp_trans, BMP085_SLAVE_ADDR, 1, 3);
- baro_bmp_status = BARO_BMP_READ_PRESS;
- }
-#else // SITL
- baro_bmp_altitude = gps.hmsl / 1000.0;
- baro_bmp_pressure = baro_bmp_altitude; //FIXME do a proper scaling here
- baro_bmp_valid = TRUE;
-#endif
}
-void baro_bmp_event( void ) {
+void baro_bmp_periodic(void) {
- if (bmp_trans.status == I2CTransSuccess) {
+ if (baro_bmp.initialized)
+ bmp085_periodic(&baro_bmp);
+ else
+ bmp085_read_eeprom_calib(&baro_bmp);
- if (baro_bmp_status == BARO_BMP_UNINIT) {
- /* get calibration data */
- bmp_ac1 = (bmp_trans.buf[0] << 8) | bmp_trans.buf[1];
- bmp_ac2 = (bmp_trans.buf[2] << 8) | bmp_trans.buf[3];
- bmp_ac3 = (bmp_trans.buf[4] << 8) | bmp_trans.buf[5];
- bmp_ac4 = (bmp_trans.buf[6] << 8) | bmp_trans.buf[7];
- bmp_ac5 = (bmp_trans.buf[8] << 8) | bmp_trans.buf[9];
- bmp_ac6 = (bmp_trans.buf[10] << 8) | bmp_trans.buf[11];
- bmp_b1 = (bmp_trans.buf[12] << 8) | bmp_trans.buf[13];
- bmp_b2 = (bmp_trans.buf[14] << 8) | bmp_trans.buf[15];
- bmp_mb = (bmp_trans.buf[16] << 8) | bmp_trans.buf[17];
- bmp_mc = (bmp_trans.buf[18] << 8) | bmp_trans.buf[19];
- bmp_md = (bmp_trans.buf[20] << 8) | bmp_trans.buf[21];
- baro_bmp_status = BARO_BMP_IDLE;
- }
- else if (baro_bmp_status == BARO_BMP_READ_TEMP) {
- /* get uncompensated temperature */
- bmp_ut = (bmp_trans.buf[0] << 8) | bmp_trans.buf[1];
- /* start high res pressure measurement */
- bmp_trans.buf[0] = BMP085_CTRL_REG;
- bmp_trans.buf[1] = BMP085_START_P3;
- i2c_transmit(&BMP_I2C_DEV, &bmp_trans, BMP085_SLAVE_ADDR, 2);
- baro_bmp_status = BARO_BMP_START_PRESS;
- }
- else if (baro_bmp_status == BARO_BMP_READ_PRESS) {
- int32_t bmp_p, bmp_t;
- int32_t bmp_x1, bmp_x2, bmp_x3;
- int32_t bmp_b3, bmp_b5, bmp_b6;
- uint32_t bmp_b4, bmp_b7;
+}
- /* get uncompensated pressure, oss=3 */
- bmp_up = (bmp_trans.buf[0] << 11) |
- (bmp_trans.buf[1] << 3) |
- (bmp_trans.buf[2] >> 5);
- /* start temp measurement */
- bmp_trans.buf[0] = BMP085_CTRL_REG;
- bmp_trans.buf[1] = BMP085_START_TEMP;
- i2c_transmit(&BMP_I2C_DEV, &bmp_trans, BMP085_SLAVE_ADDR, 2);
- baro_bmp_status = BARO_BMP_START_TEMP;
+void baro_bmp_event(void) {
- /* compensate temperature */
- bmp_x1 = (bmp_ut - bmp_ac6) * bmp_ac5 / (1<<15);
- bmp_x2 = bmp_mc * (1<<11) / (bmp_x1 + bmp_md);
- bmp_b5 = bmp_x1 + bmp_x2;
- bmp_t = (bmp_b5 + 8) / (1<<4);
+ bmp085_event(&baro_bmp);
- /* compensate pressure */
- bmp_b6 = bmp_b5 - 4000;
- bmp_x1 = (bmp_b2 * (bmp_b6 * bmp_b6 / (1<<12))) / (1<<11);
- bmp_x2 = bmp_ac2 *bmp_b6 / (1<<11);
- bmp_x3 = bmp_x1 + bmp_x2;
- bmp_b3 = (((bmp_ac1 * 4 + bmp_x3) << 3) + 2) / 4;
- bmp_x1 = bmp_ac3 * bmp_b6 / (1<<13);
- bmp_x2 = (bmp_b1 * (bmp_b6 * bmp_b6 / (1<<12))) / (1<<16);
- bmp_x3 = ((bmp_x1 + bmp_x2) +2) / (1<<2);
- bmp_b4 = bmp_ac4 * (uint32_t)(bmp_x3 + 32768) / (1<<15);
- bmp_b7 = ((uint32_t)bmp_up - bmp_b3) * (50000>>3);
- if (bmp_b7 < 0x80000000)
- bmp_p = (bmp_b7 * 2) / bmp_b4;
- else
- bmp_p = (bmp_b7 / bmp_b4) * 2;
- bmp_x1 = (bmp_p / (1<<8)) * (bmp_p / (1<<8));
- bmp_x1 = (bmp_x1 * 3038) / (1<<16);
- bmp_x2 = (-7357 * bmp_p) / (1<<16);
- bmp_p = bmp_p + (bmp_x1 + bmp_x2 + 3791) / (1<<4);
+ if (baro_bmp.data_available) {
- baro_bmp_temperature = bmp_t;
- baro_bmp_pressure = bmp_p;
-
- tmp_float = bmp_p/101325.0; //pressao nivel mar
- tmp_float = pow(tmp_float,0.190295); //eleva pressao ao expoente
- baro_bmp = 44330*(1.0-tmp_float);
-
- if (!baro_bmp_offset_init) {
- baro_bmp_offset = baro_bmp;
- baro_bmp_offset_init = TRUE;
-#if 0
- --baro_bmp_cnt;
- // Check if averaging completed
- if (baro_bmp_cnt == 0) {
- // Calculate average
- baro_bmp_offset = (baro_bmp_offset_tmp / BARO_BMP_OFFSET_NBSAMPLES_AVRG);
- // Limit offset
- if (baro_bmp_offset < BARO_BMP_OFFSET_MIN)
- baro_bmp_offset = BARO_BMP_OFFSET_MIN;
- if (baro_bmp_offset > BARO_BMP_OFFSET_MAX)
- baro_bmp_offset = BARO_BMP_OFFSET_MAX;
- baro_bmp_offset_init = TRUE;
- }
- // Check if averaging needs to continue
- else if (baro_bmp_cnt <= BARO_BMP_OFFSET_NBSAMPLES_AVRG)
- baro_bmp_offset_tmp += baro_bmp;
-#endif
- } //baro offset init
-
- baro_bmp_temp = (baro_bmp - baro_bmp_offset);
-
- if (baro_bmp_offset_init) {
- baro_bmp_altitude = ground_alt + baro_bmp_temp;
- // New value available
- baro_bmp_valid = TRUE;
+ float tmp = baro_bmp.pressure / 101325.0; // pressure at sea level
+ tmp = pow(tmp, 0.190295);
+ baro_bmp_alt = 44330 * (1.0 - tmp);
#ifdef SENSOR_SYNC_SEND
- DOWNLINK_SEND_BMP_STATUS(DefaultChannel, DefaultDevice, &bmp_up, &bmp_ut, &bmp_p, &bmp_t);
+ DOWNLINK_SEND_BMP_STATUS(DefaultChannel, DefaultDevice, &baro_bmp.up,
+ &baro_bmp.ut, &baro_bmp.pressure,
+ &baro_bmp.temperature);
#else
- RunOnceEvery(10, DOWNLINK_SEND_BMP_STATUS(DefaultChannel, DefaultDevice, &baro_bmp_temp, &bmp_ut, &bmp_p, &bmp_t));
+ RunOnceEvery(10, DOWNLINK_SEND_BMP_STATUS(DefaultChannel, DefaultDevice,
+ &baro_bmp.up, &baro_bmp.ut,
+ &baro_bmp.pressure,
+ &baro_bmp.temperature));
#endif
- } else {
- baro_bmp_altitude = 0.0;
- }
- }
}
}
diff --git a/sw/airborne/modules/sensors/baro_bmp.h b/sw/airborne/modules/sensors/baro_bmp.h
index 53adeb184d..db1039e817 100644
--- a/sw/airborne/modules/sensors/baro_bmp.h
+++ b/sw/airborne/modules/sensors/baro_bmp.h
@@ -1,56 +1,56 @@
+/*
+ * Copyright (C) 2010 Martin Mueller
+ * Copyright (C) 2013 Felix Ruess
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/**
+ * @file modules/sensors/baro_bmp.h
+ * Bosch BMP085 I2C sensor interface.
+ *
+ * This reads the values for pressure and temperature from the Bosch BMP085 sensor through I2C.
+ */
+
#ifndef BARO_BMP_H
#define BARO_BMP_H
-#include "std.h"
+#include "peripherals/bmp085.h"
-#define BMP085_EEPROM_AC1 0xAA
-#define BMP085_EEPROM_AC2 0xAC
-#define BMP085_EEPROM_AC3 0xAE
-#define BMP085_EEPROM_AC4 0xB0
-#define BMP085_EEPROM_AC5 0xB2
-#define BMP085_EEPROM_AC6 0xB4
-#define BMP085_EEPROM_B1 0xB6
-#define BMP085_EEPROM_B2 0xB8
-#define BMP085_EEPROM_MB 0xBA
-#define BMP085_EEPROM_MC 0xBC
-#define BMP085_EEPROM_MD 0xBE
+extern struct Bmp085 baro_bmp;
-#define BMP085_CTRL_REG 0xF4
+/// new measurement every 3rd baro_bmp_periodic
+#ifndef SITL
+#define BARO_BMP_DT (BARO_BMP_PERIODIC_PERIOD / 3)
+#else
+#define BARO_BMP_DT BARO_BMP_PERIODIC_PERIOID
+#endif
-#define BMP085_START_TEMP 0x2E
-#define BMP085_START_P0 0x34
-#define BMP085_START_P1 0x74
-#define BMP085_START_P2 0xB4
-#define BMP085_START_P3 0xF4
-
-#define BMP085_DAT_MSB 0xF6
-#define BMP085_DAT_LSB 0xF7
-#define BMP085_DAT_XLSB 0xF8
-
-#define BARO_BMP_UNINIT 0
-#define BARO_BMP_IDLE 1
-#define BARO_BMP_START_TEMP 2
-#define BARO_BMP_READ_TEMP 3
-#define BARO_BMP_START_PRESS 4
-#define BARO_BMP_READ_PRESS 5
-
-#define BARO_BMP_DT 0.05
extern bool_t baro_bmp_enabled;
extern float baro_bmp_r;
extern float baro_bmp_sigma2;
-
-extern uint8_t baro_bmp_status;
-extern bool_t baro_bmp_valid;
-extern uint32_t baro_bmp_pressure;
-extern uint16_t baro_bmp_temperature;
-extern int32_t baro_bmp_altitude;
-extern int32_t baro_bmp;
-extern int32_t baro_bmp_offset;
+extern int32_t baro_bmp_alt;
void baro_bmp_init(void);
void baro_bmp_periodic(void);
void baro_bmp_event(void);
-#define BaroBmpUpdate(_b) { if (baro_bmp_valid) { _b = baro_bmp_pressure; baro_bmp_valid = FALSE; } }
+#define BaroBmpUpdate(_b, _h) { if (baro_bmp.data_available) { _b = baro_bmp.pressure; _h(); baro_bmp.data_available = FALSE; } }
#endif
diff --git a/sw/airborne/modules/sensors/baro_board_module.h b/sw/airborne/modules/sensors/baro_board_module.h
index 3ea1e5682a..bc5f48d23b 100644
--- a/sw/airborne/modules/sensors/baro_board_module.h
+++ b/sw/airborne/modules/sensors/baro_board_module.h
@@ -50,16 +50,20 @@
#define BARO_DIFF_EVENT NoBaro
#endif
-#define NoBaro(_b) {}
+#define NoBaro(_b, _h) {}
/** BaroEvent macro.
* Need to be maped to one the external baro running has a module
+ *
+ * Undef if necessary (already defined in a baro_board.h file)
*/
+#ifdef BaroEvent
+#undef BaroEvent
+#endif
+
#define BaroEvent(_b_abs_handler, _b_diff_handler) { \
- BARO_ABS_EVENT(baro.absolute); \
- BARO_DIFF_EVENT(baro.differential); \
- _b_abs_handler(); \
- _b_diff_handler(); \
+ BARO_ABS_EVENT(baro.absolute, _b_abs_handler); \
+ BARO_DIFF_EVENT(baro.differential, _b_diff_handler); \
}
diff --git a/sw/airborne/modules/sensors/baro_ets.h b/sw/airborne/modules/sensors/baro_ets.h
index c492ef8fc4..33f7bbd592 100644
--- a/sw/airborne/modules/sensors/baro_ets.h
+++ b/sw/airborne/modules/sensors/baro_ets.h
@@ -45,7 +45,8 @@
#include "std.h"
#include "mcu_periph/i2c.h"
-#define BARO_ETS_DT 0.05
+/// new measurement every baro_ets_read_periodic
+#define BARO_ETS_DT BARO_ETS_READ_PERIODIC_PERIOD
extern uint16_t baro_ets_adc;
extern uint16_t baro_ets_offset;
@@ -63,6 +64,6 @@ extern void baro_ets_read_event( void );
#define BaroEtsEvent() { if (baro_ets_i2c_trans.status == I2CTransSuccess) baro_ets_read_event(); }
-#define BaroEtsUpdate(_b) { if (baro_ets_valid) { _b = baro_ets_adc; baro_ets_valid = FALSE; } }
+#define BaroEtsUpdate(_b, _h) { if (baro_ets_valid) { _b = baro_ets_adc; _h(); baro_ets_valid = FALSE; } }
#endif // BARO_ETS_H
diff --git a/sw/airborne/modules/sensors/baro_ms5611_i2c.c b/sw/airborne/modules/sensors/baro_ms5611_i2c.c
index 91d2deecbc..63be57aaa3 100644
--- a/sw/airborne/modules/sensors/baro_ms5611_i2c.c
+++ b/sw/airborne/modules/sensors/baro_ms5611_i2c.c
@@ -1,5 +1,5 @@
/*
- * Copyright (C) 2011 Martin Mueller
+ * Copyright (C) 2011-2013 The Paparazzi Team
*
* This file is part of paparazzi.
*
@@ -30,11 +30,9 @@
#include "modules/sensors/baro_ms5611_i2c.h"
#include "mcu_periph/sys_time.h"
-#include "mcu_periph/i2c.h"
#include "mcu_periph/uart.h"
#include "messages.h"
#include "subsystems/datalink/downlink.h"
-#include "subsystems/nav.h"
#ifndef MS5611_I2C_DEV
@@ -46,202 +44,73 @@
#define MS5611_SLAVE_ADDR 0xEE
#endif
+struct Ms5611_I2c baro_ms5611;
-struct i2c_transaction ms5611_trans;
-uint8_t ms5611_status;
-uint16_t ms5611_c[PROM_NB];
-uint32_t ms5611_d1, ms5611_d2;
-int32_t prom_cnt;
-int64_t baroms;
float fbaroms, ftempms;
float baro_ms5611_alt;
+bool_t baro_ms5611_alt_valid;
bool_t baro_ms5611_enabled;
-bool_t baro_ms5611_valid;
+
float baro_ms5611_r;
float baro_ms5611_sigma2;
-static int8_t baro_ms5611_crc(uint16_t* prom) {
- int32_t i, j;
- uint32_t res = 0;
- uint8_t crc = prom[7] & 0xF;
- prom[7] &= 0xFF00;
- for (i = 0; i < 16; i++) {
- if (i & 1) res ^= ((prom[i>>1]) & 0x00FF);
- else res ^= (prom[i>>1]>>8);
- for (j = 8; j > 0; j--) {
- if (res & 0x8000) res ^= 0x1800;
- res <<= 1;
- }
- }
- prom[7] |= crc;
- if (crc == ((res >> 12) & 0xF)) return 0;
- else return -1;
-}
void baro_ms5611_init(void) {
+ ms5611_i2c_init(&baro_ms5611, &MS5611_I2C_DEV, MS5611_SLAVE_ADDR);
+
baro_ms5611_enabled = TRUE;
- baro_ms5611_valid = FALSE;
- ms5611_status = MS5611_UNINIT;
+ baro_ms5611_alt_valid = FALSE;
+
baro_ms5611_r = BARO_MS5611_R;
baro_ms5611_sigma2 = BARO_MS5611_SIGMA2;
- prom_cnt = 0;
}
-void baro_ms5611_periodic( void ) {
- if (sys_time.nb_sec > 1) {
- if (ms5611_status >= MS5611_IDLE) {
- /* start D1 conversion */
- ms5611_status = MS5611_CONV_D1;
- ms5611_trans.buf[0] = MS5611_START_CONV_D1;
- i2c_transmit(&MS5611_I2C_DEV, &ms5611_trans, MS5611_SLAVE_ADDR, 1);
- RunOnceEvery((4*30), DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice,
- &ms5611_c[0], &ms5611_c[1], &ms5611_c[2], &ms5611_c[3],
- &ms5611_c[4], &ms5611_c[5], &ms5611_c[6], &ms5611_c[7]));
- }
- else if (ms5611_status == MS5611_UNINIT) {
- /* reset sensor */
- ms5611_status = MS5611_RESET;
- ms5611_trans.buf[0] = MS5611_SOFT_RESET;
- i2c_transmit(&MS5611_I2C_DEV, &ms5611_trans, MS5611_SLAVE_ADDR, 1);
- }
- else if (ms5611_status == MS5611_RESET_OK) {
- /* start getting prom data */
- ms5611_status = MS5611_PROM;
- ms5611_trans.buf[0] = MS5611_PROM_READ | (prom_cnt << 1);
- i2c_transceive(&MS5611_I2C_DEV, &ms5611_trans, MS5611_SLAVE_ADDR, 1, 2);
- }
- }
+void baro_ms5611_periodic_check( void ) {
+
+ ms5611_i2c_periodic_check(&baro_ms5611);
+
+#if BARO_MS5611_SEND_COEFF
+ // send coeff every 5s
+ RunOnceEvery((5*BARO_MS5611_PERIODIC_CHECK_FREQUENCY), baro_ms5611_send_coeff());
+#endif
}
-void baro_ms5611_d1( void ) {
+/// trigger new measurement or initialize if needed
+void baro_ms5611_read(void) {
if (sys_time.nb_sec > 1) {
- if (ms5611_status == MS5611_CONV_D1_OK) {
- /* read D1 adc */
- ms5611_status = MS5611_ADC_D1;
- ms5611_trans.buf[0] = MS5611_ADC_READ;
- i2c_transceive(&MS5611_I2C_DEV, &ms5611_trans, MS5611_SLAVE_ADDR, 1, 3);
- }
- }
-}
-
-void baro_ms5611_d2( void ) {
- if (sys_time.nb_sec > 1) {
- if (ms5611_status == MS5611_CONV_D2_OK) {
- /* read D2 adc */
- ms5611_status = MS5611_ADC_D2;
- ms5611_trans.buf[0] = MS5611_ADC_READ;
- i2c_transceive(&MS5611_I2C_DEV, &ms5611_trans, MS5611_SLAVE_ADDR, 1, 3);
- }
+ ms5611_i2c_read(&baro_ms5611);
}
}
void baro_ms5611_event( void ) {
- if (ms5611_trans.status == I2CTransSuccess) {
- switch (ms5611_status) {
- case MS5611_RESET:
- ms5611_status = MS5611_RESET_OK;
- ms5611_trans.status = I2CTransDone;
- break;
+ ms5611_i2c_event(&baro_ms5611);
- case MS5611_PROM:
- /* read prom data */
- ms5611_c[prom_cnt++] = (ms5611_trans.buf[0] << 8) | ms5611_trans.buf[1];
- if (prom_cnt < PROM_NB) {
- /* get next prom data */
- ms5611_trans.buf[0] = MS5611_PROM_READ | (prom_cnt << 1);
- i2c_transceive(&MS5611_I2C_DEV, &ms5611_trans, MS5611_SLAVE_ADDR, 1, 2);
- }
- else {
- /* done reading prom */
- ms5611_trans.status = I2CTransDone;
- /* check prom crc */
- if (baro_ms5611_crc(ms5611_c) == 0) {
- DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice,
- &ms5611_c[0], &ms5611_c[1], &ms5611_c[2], &ms5611_c[3],
- &ms5611_c[4], &ms5611_c[5], &ms5611_c[6], &ms5611_c[7]);
- ms5611_status = MS5611_IDLE;
- }
- else {
- /* checksum error, try again */
- baro_ms5611_init();
- }
- }
- break;
-
- case MS5611_CONV_D1:
- ms5611_status = MS5611_CONV_D1_OK;
- ms5611_trans.status = I2CTransDone;
- break;
-
- case MS5611_ADC_D1:
- /* read D1 (pressure) */
- ms5611_d1 = (ms5611_trans.buf[0] << 16) |
- (ms5611_trans.buf[1] << 8) |
- ms5611_trans.buf[2];
- /* start D2 conversion */
- ms5611_status = MS5611_CONV_D2;
- ms5611_trans.buf[0] = MS5611_START_CONV_D2;
- i2c_transmit(&MS5611_I2C_DEV, &ms5611_trans, MS5611_SLAVE_ADDR, 1);
- break;
-
- case MS5611_CONV_D2:
- ms5611_status = MS5611_CONV_D2_OK;
- ms5611_trans.status = I2CTransDone;
- break;
-
- case MS5611_ADC_D2: {
- int64_t dt, tempms, off, sens, t2, off2, sens2;
- /* read D2 (temperature) */
- ms5611_d2 = (ms5611_trans.buf[0] << 16) |
- (ms5611_trans.buf[1] << 8) |
- ms5611_trans.buf[2];
- ms5611_status = MS5611_IDLE;
- ms5611_trans.status = I2CTransDone;
-
- /* difference between actual and ref temperature */
- dt = ms5611_d2 - (int64_t)ms5611_c[5] * (1<<8);
- /* actual temperature */
- tempms = 2000 + ((int64_t)dt * ms5611_c[6]) / (1<<23);
- /* offset at actual temperature */
- off = ((int64_t)ms5611_c[2] * (1<<16)) + ((int64_t)ms5611_c[4] * dt) / (1<<7);
- /* sensitivity at actual temperature */
- sens = ((int64_t)ms5611_c[1] * (1<<15)) + ((int64_t)ms5611_c[3] * dt) / (1<<8);
- /* second order temperature compensation */
- if (tempms < 2000) {
- t2 = (dt*dt) / (1<<31);
- off2 = 5 * ((int64_t)(tempms-2000)*(tempms-2000)) / (1<<1);
- sens2 = 5 * ((int64_t)(tempms-2000)*(tempms-2000)) / (1<<2);
- if (tempms < -1500) {
- off2 = off2 + 7 * (int64_t)(tempms+1500)*(tempms+1500);
- sens2 = sens2 + 11 * ((int64_t)(tempms+1500)*(tempms+1500)) / (1<<1);
- }
- tempms = tempms - t2;
- off = off - off2;
- sens = sens - sens2;
- }
- /* temperature compensated pressure */
- baroms = (((int64_t)ms5611_d1 * sens) / (1<<21) - off) / (1<<15);
-
- float tmp_float = baroms/101325.0; //pressure at sea level
- tmp_float = pow(tmp_float,0.190295); //eleva pressao ao expoente
- baro_ms5611_alt = 44330*(1.0-tmp_float); //altitude above MSL
-
- baro_ms5611_valid = TRUE;
+ if (baro_ms5611.data_available) {
+ float tmp_float = baro_ms5611.data.pressure / 101325.0; //pressure at sea level
+ tmp_float = pow(tmp_float, 0.190295);
+ baro_ms5611_alt = 44330 * (1.0 - tmp_float); //altitude above MSL
+ baro_ms5611_alt_valid = TRUE;
#ifdef SENSOR_SYNC_SEND
- ftempms = tempms / 100.;
- fbaroms = baroms / 100.;
- DOWNLINK_SEND_BARO_MS5611(DefaultChannel, DefaultDevice,
- &ms5611_d1, &ms5611_d2, &fbaroms, &ftempms);
+ ftempms = baro_ms5611.data.temperature / 100.;
+ fbaroms = baro_ms5611.data.pressure / 100.;
+ DOWNLINK_SEND_BARO_MS5611(DefaultChannel, DefaultDevice,
+ &baro_ms5611.data.d1, &baro_ms5611.data.d2, &fbaroms, &ftempms);
#endif
-
- break;
- }
-
- default:
- ms5611_trans.status = I2CTransDone;
- break;
- }
+ }
+}
+
+void baro_ms5611_send_coeff(void) {
+ if (baro_ms5611.initialized) {
+ DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice,
+ &baro_ms5611.data.c[0],
+ &baro_ms5611.data.c[1],
+ &baro_ms5611.data.c[2],
+ &baro_ms5611.data.c[3],
+ &baro_ms5611.data.c[4],
+ &baro_ms5611.data.c[5],
+ &baro_ms5611.data.c[6],
+ &baro_ms5611.data.c[7]);
}
}
diff --git a/sw/airborne/modules/sensors/baro_ms5611_i2c.h b/sw/airborne/modules/sensors/baro_ms5611_i2c.h
index cad37d60db..549b6bf731 100644
--- a/sw/airborne/modules/sensors/baro_ms5611_i2c.h
+++ b/sw/airborne/modules/sensors/baro_ms5611_i2c.h
@@ -2,46 +2,29 @@
#define BARO_MS56111_I2C_H
#include "std.h"
+#include "peripherals/ms5611_i2c.h"
-/* we use OSR=4096 for maximum resolution */
-#define MS5611_SOFT_RESET 0x1E
-#define MS5611_PROM_READ 0xA0
-#define MS5611_START_CONV_D1 0x48
-#define MS5611_START_CONV_D2 0x58
-#define MS5611_ADC_READ 0x00
-
-#define PROM_NB 8
-
-#define BARO_MS5611_DT 0.05
+/// new measurement with every baro_ms5611_read() call
+#define BARO_MS5611_DT BARO_MS5611_READ_PERIOD
#define BARO_MS5611_R 20
#define BARO_MS5611_SIGMA2 1
-extern float baro_ms5611_alt;
-extern bool_t baro_ms5611_valid;
-extern bool_t baro_ms5611_enabled;
extern float baro_ms5611_r;
extern float baro_ms5611_sigma2;
-extern int64_t baroms;
-enum ms5611_stat{
- MS5611_UNINIT,
- MS5611_RESET,
- MS5611_RESET_OK,
- MS5611_PROM,
- MS5611_IDLE,
- MS5611_CONV_D1,
- MS5611_CONV_D1_OK,
- MS5611_ADC_D1,
- MS5611_CONV_D2,
- MS5611_CONV_D2_OK,
- MS5611_ADC_D2
-};
+extern float baro_ms5611_alt;
+extern bool_t baro_ms5611_alt_valid;
+extern bool_t baro_ms5611_enabled;
+
+extern struct Ms5611_I2c baro_ms5611;
extern void baro_ms5611_init(void);
-extern void baro_ms5611_periodic(void);
-extern void baro_ms5611_d1(void);
-extern void baro_ms5611_d2(void);
+extern void baro_ms5611_read(void);
+extern void baro_ms5611_periodic_check(void);
extern void baro_ms5611_event(void);
+extern void baro_ms5611_send_coeff(void);
-#define BaroMs5611Update(_b) { if (baro_ms5611_valid) { _b = baro_ms5611_alt; baro_ms5611_valid = FALSE; } }
+#define BaroMs5611UpdatePressure(_b, _h) { if (baro_ms5611.data_available) { _b = baro_ms5611.data.pressure; _h(); baro_ms5611.data_available = FALSE; } }
+
+#define BaroMs5611UpdateAlt(_b, _h) { if (baro_ms5611.data_available) { _b = baro_ms5611_alt; _h(); baro_ms5611.data_available = FALSE; } }
#endif
diff --git a/sw/airborne/modules/sensors/baro_ms5611_spi.c b/sw/airborne/modules/sensors/baro_ms5611_spi.c
new file mode 100644
index 0000000000..7da2575f41
--- /dev/null
+++ b/sw/airborne/modules/sensors/baro_ms5611_spi.c
@@ -0,0 +1,120 @@
+/*
+ * Copyright (C) 2011-2013 The Paparazzi Team
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/**
+ * @file modules/sensors/baro_ms5611_spi.c
+ * Measurement Specialties (Intersema) MS5611-01BA pressure/temperature sensor interface for SPI.
+ *
+ */
+
+
+#include "modules/sensors/baro_ms5611_spi.h"
+
+#include "mcu_periph/sys_time.h"
+#include "mcu_periph/uart.h"
+#include "messages.h"
+#include "subsystems/datalink/downlink.h"
+
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+
+#ifndef MS5611_SPI_DEV
+#define MS5611_SPI_DEV spi1
+#endif
+
+#ifndef MS5611_SLAVE_DEV
+#define MS5611_SLAVE_DEV SPI_SLAVE0
+#endif
+
+
+struct Ms5611_Spi baro_ms5611;
+
+float fbaroms, ftempms;
+float baro_ms5611_alt;
+bool_t baro_ms5611_alt_valid;
+bool_t baro_ms5611_enabled;
+
+float baro_ms5611_r;
+float baro_ms5611_sigma2;
+
+
+void baro_ms5611_init(void) {
+ ms5611_spi_init(&baro_ms5611, &MS5611_SPI_DEV, MS5611_SLAVE_DEV);
+
+ baro_ms5611_enabled = TRUE;
+ baro_ms5611_alt_valid = FALSE;
+
+ baro_ms5611_r = BARO_MS5611_R;
+ baro_ms5611_sigma2 = BARO_MS5611_SIGMA2;
+}
+
+void baro_ms5611_periodic_check( void ) {
+
+ ms5611_spi_periodic_check(&baro_ms5611);
+
+#if BARO_MS5611_SEND_COEFF
+ // send coeff every 5s
+ RunOnceEvery((5*BARO_MS5611_PERIODIC_CHECK_FREQUENCY), baro_ms5611_send_coeff());
+#endif
+
+}
+
+/// trigger new measurement or initialize if needed
+void baro_ms5611_read(void) {
+ if (sys_time.nb_sec > 1) {
+ ms5611_spi_read(&baro_ms5611);
+ }
+}
+
+void baro_ms5611_event( void ) {
+
+ ms5611_spi_event(&baro_ms5611);
+
+ if (baro_ms5611.data_available) {
+ float tmp_float = baro_ms5611.data.pressure / 101325.0; //pressure at sea level
+ tmp_float = pow(tmp_float, 0.190295);
+ baro_ms5611_alt = 44330 * (1.0 - tmp_float); //altitude above MSL
+ baro_ms5611_alt_valid = TRUE;
+
+#ifdef SENSOR_SYNC_SEND
+ ftempms = baro_ms5611.data.temperature / 100.;
+ fbaroms = baro_ms5611.data.pressure / 100.;
+ DOWNLINK_SEND_BARO_MS5611(DefaultChannel, DefaultDevice,
+ &baro_ms5611.data.d1, &baro_ms5611.data.d2, &fbaroms, &ftempms);
+#endif
+ }
+}
+
+void baro_ms5611_send_coeff(void) {
+ if (baro_ms5611.initialized) {
+ DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice,
+ &baro_ms5611.data.c[0],
+ &baro_ms5611.data.c[1],
+ &baro_ms5611.data.c[2],
+ &baro_ms5611.data.c[3],
+ &baro_ms5611.data.c[4],
+ &baro_ms5611.data.c[5],
+ &baro_ms5611.data.c[6],
+ &baro_ms5611.data.c[7]);
+ }
+}
diff --git a/sw/airborne/modules/sensors/baro_ms5611_spi.h b/sw/airborne/modules/sensors/baro_ms5611_spi.h
new file mode 100644
index 0000000000..7be9f9c2e5
--- /dev/null
+++ b/sw/airborne/modules/sensors/baro_ms5611_spi.h
@@ -0,0 +1,58 @@
+/*
+ * 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.
+ *
+ */
+
+/**
+ * @file modules/sensors/baro_ms5611_spi.h
+ * Measurement Specialties (Intersema) MS5611-01BA pressure/temperature sensor interface for SPI.
+ *
+ */
+
+#ifndef BARO_MS56111_SPI_H
+#define BARO_MS56111_SPI_H
+
+#include "std.h"
+#include "peripherals/ms5611_spi.h"
+
+/// new measurement with every baro_ms5611_read() call
+#define BARO_MS5611_DT BARO_MS5611_READ_PERIOD
+#define BARO_MS5611_R 20
+#define BARO_MS5611_SIGMA2 1
+extern float baro_ms5611_r;
+extern float baro_ms5611_sigma2;
+
+extern float baro_ms5611_alt;
+extern bool_t baro_ms5611_alt_valid;
+extern bool_t baro_ms5611_enabled;
+
+extern struct Ms5611_Spi baro_ms5611;
+
+extern void baro_ms5611_init(void);
+extern void baro_ms5611_read(void);
+extern void baro_ms5611_periodic_check(void);
+extern void baro_ms5611_event(void);
+extern void baro_ms5611_send_coeff(void);
+
+#define BaroMs5611UpdatePressure(_b, _h) { if (baro_ms5611.data_available) { _b = baro_ms5611.data.pressure; _h(); baro_ms5611.data_available = FALSE; } }
+
+#define BaroMs5611UpdateAlt(_b, _h) { if (baro_ms5611.data_available) { _b = baro_ms5611_alt; _h(); baro_ms5611.data_available = FALSE; } }
+
+#endif
diff --git a/sw/airborne/modules/sensors/baro_scp.h b/sw/airborne/modules/sensors/baro_scp.h
index 2619adb96d..77aeb504d1 100644
--- a/sw/airborne/modules/sensors/baro_scp.h
+++ b/sw/airborne/modules/sensors/baro_scp.h
@@ -20,6 +20,6 @@ void baro_scp_init(void);
void baro_scp_periodic(void);
void baro_scp_event(void);
-#define BaroScpUpdate(_b) { if (baro_scp_available) { _b = baro_scp_pressure; baro_scp_available = FALSE; } }
+#define BaroScpUpdate(_b, _h) { if (baro_scp_available) { _b = baro_scp_pressure; _h(); baro_scp_available = FALSE; } }
#endif
diff --git a/sw/airborne/modules/time/flight_time.c b/sw/airborne/modules/time/flight_time.c
new file mode 100644
index 0000000000..16328c7c84
--- /dev/null
+++ b/sw/airborne/modules/time/flight_time.c
@@ -0,0 +1,46 @@
+/*
+ * Copyright (C) 2013 Elisabeth van der Sman, 2013 Freek van Tienen
+ *
+ * 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 modules/time/flight_time.c
+ *
+ * Flight time counter that can be set from the gcs
+ */
+
+#include "flight_time.h"
+#include "generated/airframe.h"
+
+uint16_t time_until_land;
+
+#ifndef FLIGHT_TIME_LEFT
+#define FLIGHT_TIME_LEFT 10000
+#endif
+
+void flight_time_init(void) {
+ time_until_land = FLIGHT_TIME_LEFT;
+}
+
+void flight_time_periodic( void ) {
+ // Count downwards
+ if(time_until_land > 0)
+ time_until_land--;
+}
diff --git a/sw/airborne/arch/omap/subsystems/electrical/electrical_arch.c b/sw/airborne/modules/time/flight_time.h
similarity index 69%
rename from sw/airborne/arch/omap/subsystems/electrical/electrical_arch.c
rename to sw/airborne/modules/time/flight_time.h
index 52f1e41ef7..2a4ff41f89 100644
--- a/sw/airborne/arch/omap/subsystems/electrical/electrical_arch.c
+++ b/sw/airborne/modules/time/flight_time.h
@@ -1,6 +1,5 @@
/*
- *
- * Copyright (C) 2009-2013 The Paparazzi Team
+ * Copyright (C) 2013 Elisabeth van der Sman, 2013 Freek van Tienen
*
* This file is part of paparazzi.
*
@@ -22,16 +21,19 @@
*/
/**
- * @file arch/omap/subsystems/electrical/electrical_arch.c
- * arch specific electrical status readings
+ * @file modules/time/flight_time.h
+ *
+ * Flight time counter that can be set from the gcs
*/
-#include "subsystems/electrical/electrical_arch.h"
+#ifndef FLIGHT_TIME_H
+#define FLIGHT_TIME_H
-struct Electrical electrical;
+#include "std.h"
-void electrical_init(void) { }
+extern uint16_t time_until_land;
-void electrical_periodic(void) {
- electrical.vsupply = 120;
-}
+void flight_time_init(void);
+void flight_time_periodic(void);
+
+#endif /* FLIGHT_TIME_H */
diff --git a/sw/airborne/peripherals/bmp085.c b/sw/airborne/peripherals/bmp085.c
new file mode 100644
index 0000000000..2c123ef592
--- /dev/null
+++ b/sw/airborne/peripherals/bmp085.c
@@ -0,0 +1,196 @@
+/*
+ * Copyright (C) 2010 Martin Mueller
+ * Copyright (C) 2013 Felix Ruess
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/** @file peripherals/bmp085.c
+ * Bosch BMP085 driver interface.
+ */
+
+#include "peripherals/bmp085.h"
+#include "peripherals/bmp085_regs.h"
+
+
+static int32_t bmp085_compensated_temperature(struct Bmp085Calib* calib, int32_t raw)
+{
+ int32_t x1 = (raw - calib->ac6) * calib->ac5 / (1<<15);
+ int32_t x2 = (calib->mc << 11) / (x1 + calib->md);
+ calib->b5 = x1 + x2;
+ return (calib->b5 + 8) >> 4;
+}
+
+
+
+/** Apply temp calibration and sensor calibration to raw measurement to get Pa
+ * (from BMP085 datasheet)
+ */
+static int32_t bmp085_compensated_pressure(struct Bmp085Calib* calib, int32_t raw)
+{
+ int32_t b6 = calib->b5 - 4000;
+ int32_t x1 = (calib->b2 * (b6 * b6 >> 12)) >> 11;
+ int32_t x2 = calib->ac2 * b6 >> 11;
+ int32_t x3 = x1 + x2;
+ int32_t b3 = (((calib->ac1 * 4 + x3) << BMP085_OSS) + 2)/4;
+ x1 = calib->ac3 * b6 >> 13;
+ x2 = (calib->b1 * (b6 * b6 >> 12)) >> 16;
+ x3 = ((x1 + x2) + 2) >> 2;
+ uint32_t b4 = (calib->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);
+}
+
+/**
+ * Dummy function to always return TRUE on EndOfConversion check.
+ * Ensure proper timing trough frequency of bmp085_periodic instead!
+ */
+static bool_t bmp085_eoc_true(void)
+{
+ return TRUE;
+}
+
+
+void bmp085_read_eeprom_calib(struct Bmp085* bmp)
+{
+ if (bmp->status == BMP085_STATUS_UNINIT) {
+ bmp->initialized = FALSE;
+ bmp->i2c_trans.buf[0] = BMP085_EEPROM_AC1;
+ i2c_transceive(bmp->i2c_p, &(bmp->i2c_trans), bmp->i2c_trans.slave_addr, 1, 22);
+ }
+}
+
+
+void bmp085_init(struct Bmp085* bmp, struct i2c_periph *i2c_p, uint8_t addr)
+{
+ /* set i2c_peripheral */
+ bmp->i2c_p = i2c_p;
+
+ /* slave address */
+ bmp->i2c_trans.slave_addr = addr;
+ /* set initial status: Success or Done */
+ bmp->i2c_trans.status = I2CTransDone;
+
+ bmp->data_available = FALSE;
+ bmp->initialized = FALSE;
+ bmp->status = BMP085_STATUS_UNINIT;
+
+ /* by default assign EOC function that always returns TRUE
+ * ensure proper timing through frequeny of bmp_periodic!
+ */
+ bmp->eoc = &bmp085_eoc_true;
+}
+
+/**
+ * Start new measurement if idle or read temp/pressure.
+ * Should run at < 40Hz unless eoc check function is provided.
+ * At ultra high resolution (oss = 3) conversion time is max 25.5ms.
+ */
+void bmp085_periodic(struct Bmp085* bmp)
+{
+ switch (bmp->status) {
+ case BMP085_STATUS_IDLE:
+ /* start temp measurement */
+ bmp->i2c_trans.buf[0] = BMP085_CTRL_REG;
+ bmp->i2c_trans.buf[1] = BMP085_START_TEMP;
+ i2c_transmit(bmp->i2c_p, &bmp->i2c_trans, bmp->i2c_trans.slave_addr, 2);
+ bmp->status = BMP085_STATUS_START_TEMP;
+ break;
+
+ case BMP085_STATUS_START_TEMP:
+ /* read temp measurement */
+ if (bmp->eoc()) {
+ bmp->i2c_trans.buf[0] = BMP085_DAT_MSB;
+ i2c_transceive(bmp->i2c_p, &bmp->i2c_trans, bmp->i2c_trans.slave_addr, 1, 2);
+ bmp->status = BMP085_STATUS_READ_TEMP;
+ }
+ break;
+
+ case BMP085_STATUS_START_PRESS:
+ /* read press measurement */
+ if (bmp->eoc()) {
+ bmp->i2c_trans.buf[0] = BMP085_DAT_MSB;
+ i2c_transceive(bmp->i2c_p, &bmp->i2c_trans, bmp->i2c_trans.slave_addr, 1, 3);
+ bmp->status = BMP085_STATUS_READ_PRESS;
+ }
+ break;
+
+ default:
+ break;
+ }
+}
+
+void bmp085_event(struct Bmp085* bmp)
+{
+ if (bmp->i2c_trans.status == I2CTransSuccess) {
+ switch (bmp->status) {
+ case BMP085_STATUS_UNINIT:
+ bmp->calib.ac1 = (bmp->i2c_trans.buf[0] << 8) | bmp->i2c_trans.buf[1];
+ bmp->calib.ac2 = (bmp->i2c_trans.buf[2] << 8) | bmp->i2c_trans.buf[3];
+ bmp->calib.ac3 = (bmp->i2c_trans.buf[4] << 8) | bmp->i2c_trans.buf[5];
+ bmp->calib.ac4 = (bmp->i2c_trans.buf[6] << 8) | bmp->i2c_trans.buf[7];
+ bmp->calib.ac5 = (bmp->i2c_trans.buf[8] << 8) | bmp->i2c_trans.buf[9];
+ bmp->calib.ac6 = (bmp->i2c_trans.buf[10] << 8) | bmp->i2c_trans.buf[11];
+ bmp->calib.b1 = (bmp->i2c_trans.buf[12] << 8) | bmp->i2c_trans.buf[13];
+ bmp->calib.b2 = (bmp->i2c_trans.buf[14] << 8) | bmp->i2c_trans.buf[15];
+ bmp->calib.mb = (bmp->i2c_trans.buf[16] << 8) | bmp->i2c_trans.buf[17];
+ bmp->calib.mc = (bmp->i2c_trans.buf[18] << 8) | bmp->i2c_trans.buf[19];
+ bmp->calib.md = (bmp->i2c_trans.buf[20] << 8) | bmp->i2c_trans.buf[21];
+ bmp->status = BMP085_STATUS_IDLE;
+ bmp->initialized = TRUE;
+ break;
+
+ case BMP085_STATUS_READ_TEMP:
+ /* get uncompensated temperature */
+ bmp->ut = (bmp->i2c_trans.buf[0] << 8) | bmp->i2c_trans.buf[1];
+ bmp->temperature = bmp085_compensated_temperature(&(bmp->calib), bmp->ut);
+ /* start high res pressure measurement */
+ bmp->i2c_trans.buf[0] = BMP085_CTRL_REG;
+ bmp->i2c_trans.buf[1] = BMP085_START_P3;
+ i2c_transmit(bmp->i2c_p, &bmp->i2c_trans, bmp->i2c_trans.slave_addr, 2);
+ bmp->status = BMP085_STATUS_START_PRESS;
+ break;
+
+ case BMP085_STATUS_READ_PRESS:
+ /* get uncompensated pressure */
+ bmp->up = (bmp->i2c_trans.buf[0]<<16) |
+ (bmp->i2c_trans.buf[1] << 8) |
+ bmp->i2c_trans.buf[2];
+ bmp->up = bmp->up >> (8 - BMP085_OSS);
+ bmp->pressure = bmp085_compensated_pressure(&(bmp->calib), bmp->up);
+ bmp->data_available = TRUE;
+ bmp->status = BMP085_STATUS_IDLE;
+ break;
+
+ default:
+ break;
+ }
+ }
+ else if (bmp->i2c_trans.status == I2CTransFailed) {
+ /* try again */
+ if (bmp->initialized)
+ bmp->status = BMP085_STATUS_IDLE;
+ else
+ bmp->status = BMP085_STATUS_UNINIT;
+ bmp->i2c_trans.status = I2CTransDone;
+ }
+}
diff --git a/sw/airborne/peripherals/bmp085.h b/sw/airborne/peripherals/bmp085.h
new file mode 100644
index 0000000000..a335f1e3b5
--- /dev/null
+++ b/sw/airborne/peripherals/bmp085.h
@@ -0,0 +1,81 @@
+/*
+ * Copyright (C) 2010 Martin Mueller
+ * Copyright (C) 2013 Felix Ruess
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/** @file peripherals/bmp085.h
+ * Bosch BMP085 driver interface.
+ */
+
+#ifndef BMP085_H
+#define BMP085_H
+
+#include "mcu_periph/i2c.h"
+#include "std.h"
+
+enum Bmp085Status {
+ BMP085_STATUS_UNINIT,
+ BMP085_STATUS_IDLE,
+ BMP085_STATUS_START_TEMP,
+ BMP085_STATUS_READ_TEMP,
+ BMP085_STATUS_START_PRESS,
+ BMP085_STATUS_READ_PRESS
+};
+
+struct Bmp085Calib {
+ // 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;
+};
+
+typedef bool_t (*Bmp085EOC)(void);
+
+struct Bmp085 {
+ struct i2c_periph *i2c_p;
+ struct i2c_transaction i2c_trans;
+ Bmp085EOC eoc; ///< function to check End Of Conversion
+ enum Bmp085Status status; ///< state machine status
+ bool_t initialized; ///< config done flag
+ volatile bool_t data_available; ///< data ready flag
+ struct Bmp085Calib calib;
+ int32_t ut; ///< uncompensated temperature
+ int32_t up; ///< uncompensated pressure
+ int32_t temperature; ///< temperature in 0.1 deg Celcius
+ int32_t pressure; ///< pressure in Pascal
+};
+
+extern void bmp085_read_eeprom_calib(struct Bmp085* bmp);
+extern void bmp085_init(struct Bmp085* bmp, struct i2c_periph *i2c_p, uint8_t addr);
+extern void bmp085_periodic(struct Bmp085* bmp);
+extern void bmp085_event(struct Bmp085* bmp);
+
+#endif
diff --git a/sw/airborne/peripherals/bmp085_regs.h b/sw/airborne/peripherals/bmp085_regs.h
new file mode 100644
index 0000000000..fb94b6a67d
--- /dev/null
+++ b/sw/airborne/peripherals/bmp085_regs.h
@@ -0,0 +1,59 @@
+/*
+ * Copyright (C) 2010 Martin Mueller
+ * Copyright (C) 2013 Felix Ruess
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/** @file peripherals/bmp085_regs.h
+ * Bosch BMP085 register definitions.
+ */
+
+#ifndef BMP085_REGS_H
+#define BMP085_REGS_H
+
+#define BMP085_EEPROM_AC1 0xAA
+#define BMP085_EEPROM_AC2 0xAC
+#define BMP085_EEPROM_AC3 0xAE
+#define BMP085_EEPROM_AC4 0xB0
+#define BMP085_EEPROM_AC5 0xB2
+#define BMP085_EEPROM_AC6 0xB4
+#define BMP085_EEPROM_B1 0xB6
+#define BMP085_EEPROM_B2 0xB8
+#define BMP085_EEPROM_MB 0xBA
+#define BMP085_EEPROM_MC 0xBC
+#define BMP085_EEPROM_MD 0xBE
+
+#define BMP085_CTRL_REG 0xF4
+
+#define BMP085_START_TEMP 0x2E
+#define BMP085_START_P0 0x34
+#define BMP085_START_P1 0x74
+#define BMP085_START_P2 0xB4
+#define BMP085_START_P3 0xF4
+
+#define BMP085_DAT_MSB 0xF6
+#define BMP085_DAT_LSB 0xF7
+#define BMP085_DAT_XLSB 0xF8
+
+/// Over sample setting (0-3)
+#define BMP085_OSS 3
+
+#define BMP085_SLAVE_ADDR 0xEE
+
+#endif
diff --git a/sw/airborne/peripherals/cyrf6936.c b/sw/airborne/peripherals/cyrf6936.c
new file mode 100644
index 0000000000..1b1e678a48
--- /dev/null
+++ b/sw/airborne/peripherals/cyrf6936.c
@@ -0,0 +1,420 @@
+/*
+ * Copyright (C) 2013 Freek van Tienen
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file peripherals/cyrf6936.c
+ * Driver for the cyrf6936 2.4GHz radio chip
+ */
+
+#include "cyrf6936.h"
+#include "mcu_periph/spi.h"
+#include "mcu_periph/gpio.h"
+#include "mcu_periph/sys_time.h"
+#include "subsystems/radio_control.h"
+
+#include "mcu_periph/uart.h"
+#include "messages.h"
+#include "subsystems/datalink/downlink.h"
+
+/* Static functions used in the different statuses */
+static bool_t cyrf6936_write_register(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t data);
+static bool_t cyrf6936_write_block(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t data[], const uint8_t length);
+static bool_t cyrf6936_read_register(struct Cyrf6936 *cyrf, const uint8_t addr);
+static bool_t cyrf6936_read_block(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t length);
+
+/**
+ * Initializing the cyrf chip
+ */
+void cyrf6936_init(struct Cyrf6936 *cyrf, struct spi_periph *spi_p, const uint8_t slave_idx, const uint32_t rst_port, const uint16_t rst_pin) {
+ /* Set spi_peripheral and the status */
+ cyrf->spi_p = spi_p;
+ cyrf->status = CYRF6936_UNINIT;
+ cyrf->has_irq = FALSE;
+
+ /* Set the spi transaction */
+ cyrf->spi_t.cpol = SPICpolIdleLow;
+ cyrf->spi_t.cpha = SPICphaEdge1;
+ cyrf->spi_t.dss = SPIDss8bit;
+ cyrf->spi_t.bitorder = SPIMSBFirst;
+ cyrf->spi_t.cdiv = SPIDiv64;
+
+ cyrf->spi_t.input_length = 0;
+ cyrf->spi_t.output_length = 0;
+ cyrf->spi_t.input_buf = cyrf->input_buf;
+ cyrf->spi_t.output_buf = cyrf->output_buf;
+ cyrf->spi_t.slave_idx = slave_idx;
+ cyrf->spi_t.select = SPISelectUnselect;
+ cyrf->spi_t.status = SPITransDone;
+
+ /* Reset the CYRF6936 chip (busy waiting) */
+ gpio_setup_output(rst_port, rst_pin);
+ gpio_set(rst_port, rst_pin);
+ sys_time_usleep(100);
+ gpio_clear(rst_port, rst_pin);
+ sys_time_usleep(100);
+
+ /* Get the MFG ID */
+ cyrf->status = CYRF6936_GET_MFG_ID;
+ cyrf->buffer_idx = 0;
+ cyrf6936_write_register(cyrf, CYRF_MFG_ID, 0xFF);
+}
+
+/**
+ * The on event call for the CYRF6936 chip
+ */
+void cyrf6936_event(struct Cyrf6936 *cyrf) {
+ int i;
+ // Check if cyrf is initialized
+ if(cyrf->status == CYRF6936_UNINIT)
+ return;
+
+ // Check if there is still a transaction in progress
+ if(cyrf->spi_t.status == SPITransPending || cyrf->spi_t.status == SPITransRunning)
+ return;
+
+ /* Check the status of the cyrf */
+ switch (cyrf->status) {
+
+ /* Getting the MFG id */
+ case CYRF6936_GET_MFG_ID:
+ // When the last transaction isn't failed send the next
+ if(cyrf->spi_t.status != SPITransFailed)
+ cyrf->buffer_idx++;
+
+ cyrf->spi_t.status = SPITransDone;
+
+ // Switch for the different states
+ switch (cyrf->buffer_idx) {
+ case 0:
+ cyrf6936_write_register(cyrf, CYRF_MFG_ID, 0xFF);
+ break;
+ case 1:
+ cyrf6936_read_block(cyrf, CYRF_MFG_ID, 6);
+ break;
+ case 2:
+ // Copy the MFG id
+ for(i = 0; i < 6; i++)
+ cyrf->mfg_id[i] = cyrf->input_buf[i+1];
+
+ cyrf6936_write_register(cyrf, CYRF_MFG_ID, 0x00);
+ break;
+ default:
+ cyrf->status = CYRF6936_IDLE;
+ break;
+ }
+ break;
+
+ /* Do a multi write */
+ case CYRF6936_MULTIWRITE:
+ // When the last transaction isn't failed send the next
+ if(cyrf->spi_t.status != SPITransFailed)
+ cyrf->buffer_idx++;
+
+ cyrf->spi_t.status = SPITransDone;
+
+ // When we are done writing
+ if(cyrf->buffer_idx == cyrf->buffer_length) {
+ cyrf->status = CYRF6936_IDLE;
+ break;
+ }
+
+ // Write the next register from the buffer
+ cyrf6936_write_register(cyrf,
+ ((uint8_t (*)[2])cyrf->buffer)[cyrf->buffer_idx][0],
+ ((uint8_t (*)[2])cyrf->buffer)[cyrf->buffer_idx][1]);
+ break;
+
+ /* Do a write of the data code */
+ case CYRF6936_DATA_CODE:
+ break;
+
+ /* Do a write of channel, sop, data and crc */
+ case CYRF6936_CHAN_SOP_DATA_CRC:
+ // When the last transaction isn't failed send the next
+ if(cyrf->spi_t.status != SPITransFailed)
+ cyrf->buffer_idx++;
+
+ cyrf->spi_t.status = SPITransDone;
+
+ // Switch for the different states
+ switch (cyrf->buffer_idx) {
+ case 0: // Write the CRC LSB
+ cyrf6936_write_register(cyrf, CYRF_CRC_SEED_LSB, cyrf->buffer[0]);
+ break;
+ case 1: // Write the CRC MSB
+ cyrf6936_write_register(cyrf, CYRF_CRC_SEED_MSB, cyrf->buffer[1]);
+ break;
+ case 2: // Write the SOP code
+ cyrf6936_write_block(cyrf, CYRF_SOP_CODE, &(cyrf->buffer[2]), 8);
+ break;
+ case 3: // Write the DATA code
+ cyrf6936_write_block(cyrf, CYRF_DATA_CODE, &(cyrf->buffer[10]), 16);
+ break;
+ case 4: // Write the Channel
+ cyrf6936_write_register(cyrf, CYRF_CHANNEL, cyrf->buffer[26]);
+ break;
+ default:
+ cyrf->status = CYRF6936_IDLE;
+ break;
+ }
+ break;
+
+ /* Do a read of the receive irq status, receive status and the receive packet */
+ case CYRF6936_RX_IRQ_STATUS_PACKET:
+ // When the last transaction isn't failed send the next
+ if(cyrf->spi_t.status != SPITransFailed)
+ cyrf->buffer_idx++;
+
+ cyrf->spi_t.status = SPITransDone;
+
+ // Switch for the different states
+ switch (cyrf->buffer_idx) {
+ case 0: // Read the receive IRQ status
+ cyrf6936_read_register(cyrf, CYRF_RX_IRQ_STATUS);
+ break;
+ case 1: // Read the send IRQ status
+ cyrf->rx_irq_status = cyrf->input_buf[1];
+ cyrf6936_read_register(cyrf, CYRF_TX_IRQ_STATUS);
+ break;
+ case 2: // Read the receive status
+ cyrf->tx_irq_status = cyrf->input_buf[1];
+ cyrf6936_read_register(cyrf, CYRF_RX_STATUS);
+ break;
+ case 3: // Set the packet length
+ cyrf->rx_status = cyrf->input_buf[1];
+ cyrf6936_read_register(cyrf, CYRF_RX_COUNT);
+ break;
+ case 4: // Read the receive packet
+ cyrf->rx_count = cyrf->input_buf[1];
+ cyrf6936_read_block(cyrf, CYRF_RX_BUFFER, 16);
+ break;
+ default:
+ // Copy the receive packet
+ for(i = 0; i < 16; i++)
+ cyrf->rx_packet[i] = cyrf->input_buf[i+1];
+
+ cyrf->has_irq = TRUE;
+ cyrf->status = CYRF6936_IDLE;
+ break;
+ }
+ break;
+
+ /* The CYRF6936 is busy sending a packet */
+ case CYRF6936_SEND:
+ // When the last transaction isn't failed send the next
+ if(cyrf->spi_t.status != SPITransFailed)
+ cyrf->buffer_idx++;
+
+ cyrf->spi_t.status = SPITransDone;
+
+ // Switch for the different states
+ switch (cyrf->buffer_idx) {
+ case 0: // Set the packet length
+ cyrf6936_write_register(cyrf, CYRF_TX_LENGTH, cyrf->buffer[0]);
+ break;
+ case 1: // Clear the TX buffer
+ cyrf6936_write_register(cyrf, CYRF_TX_CTRL, CYRF_TX_CLR);
+ break;
+ case 2: // Write the send packet
+ cyrf6936_write_block(cyrf, CYRF_TX_BUFFER, &cyrf->buffer[1], 16);
+ break;
+ case 3: // Send the packet
+ cyrf6936_write_register(cyrf, CYRF_TX_CTRL, CYRF_TX_GO | CYRF_TXC_IRQEN | CYRF_TXE_IRQEN);
+ break;
+ default:
+ cyrf->status = CYRF6936_IDLE;
+ break;
+ }
+ break;
+
+ /* This should not happen */
+ default:
+ break;
+ }
+}
+
+/**
+ * Write a byte to a register
+ */
+static bool_t cyrf6936_write_register(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t data) {
+ return cyrf6936_write_block(cyrf, addr, &data, 1);
+}
+
+/**
+ * Write multiple bytes to a register
+ */
+static bool_t cyrf6936_write_block(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t data[], const uint8_t length) {
+ uint8_t i;
+ /* Check if there is already a SPI transaction busy */
+ if(cyrf->spi_t.status != SPITransDone)
+ return FALSE;
+
+ /* Set the buffer and commit the transaction */
+ cyrf->spi_t.output_length = length+1;
+ cyrf->spi_t.input_length = 0;
+ cyrf->output_buf[0] = addr | CYRF_DIR;
+
+ // Copy the data
+ for(i = 0; i < length; i++)
+ cyrf->output_buf[i+1] = data[i];
+
+ // Submit the transaction
+ return spi_submit(cyrf->spi_p, &(cyrf->spi_t));
+}
+
+/**
+ * Read a byte from a register
+ */
+static bool_t cyrf6936_read_register(struct Cyrf6936 *cyrf, const uint8_t addr) {
+ return cyrf6936_read_block(cyrf, addr, 1);
+}
+
+/**
+ * Read multiple bytes from a register
+ */
+static bool_t cyrf6936_read_block(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t length) {
+ if(cyrf->spi_t.status != SPITransDone)
+ return FALSE;
+
+ /* Set the buffer and commit the transaction */
+ cyrf->spi_t.output_length = 1;
+ cyrf->spi_t.input_length = length + 1;
+ cyrf->output_buf[0] = addr;
+
+ // Submit the transaction
+ return spi_submit(cyrf->spi_p, &(cyrf->spi_t));
+}
+
+/**
+ * Write to one register
+ */
+bool_t cyrf6936_write(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t data) {
+ const uint8_t data_multi[][2] = {
+ {addr, data}
+ };
+ return cyrf6936_multi_write(cyrf, data_multi, 1);
+}
+
+/**
+ * Write to multiple registers one byte
+ */
+bool_t cyrf6936_multi_write(struct Cyrf6936 *cyrf, const uint8_t data[][2], const uint8_t length) {
+ uint8_t i;
+ /* Check if the cyrf6936 isn't busy */
+ if(cyrf->status != CYRF6936_IDLE)
+ return FALSE;
+
+ // Set the status
+ cyrf->status = CYRF6936_MULTIWRITE;
+
+ /* Set the multi write */
+ cyrf->buffer_length = length;
+ cyrf->buffer_idx = 0;
+
+ // Copy the buffer
+ for(i = 0; i< length; i++) {
+ cyrf->buffer[i*2] = data[i][0];
+ cyrf->buffer[i*2+1] = data[i][1];
+ }
+
+ /* Write the first regiter */
+ if(length > 0)
+ cyrf6936_write_register(cyrf, data[0][0], data[0][1]);
+
+ return TRUE;
+}
+
+/**
+ * Set the channel, SOP code, DATA code and the CRC seed
+ */
+bool_t cyrf6936_write_chan_sop_data_crc(struct Cyrf6936 *cyrf, const uint8_t chan, const uint8_t sop_code[], const uint8_t data_code[], const uint16_t crc_seed) {
+ uint8_t i;
+ /* Check if the cyrf6936 isn't busy */
+ if(cyrf->status != CYRF6936_IDLE)
+ return FALSE;
+
+ // Set the status
+ cyrf->status = CYRF6936_CHAN_SOP_DATA_CRC;
+
+ // Copy the CRC
+ cyrf->buffer[0] = crc_seed & 0xFF;
+ cyrf->buffer[1] = (crc_seed >> 8) & 0xFF;
+
+ // Copy the SOP code
+ for(i = 0; i < 8; i++)
+ cyrf->buffer[i+2] = sop_code[i];
+
+ // Copy the DATA code
+ for(i = 0; i < 16; i++)
+ cyrf->buffer[i+10] = data_code[i];
+
+ // Copy the channel
+ cyrf->buffer[26] = chan;
+
+ /* Try to write the CRC LSB */
+ cyrf->buffer_idx = 0;
+ cyrf6936_write_register(cyrf, CYRF_CRC_SEED_LSB, cyrf->buffer[0]);
+
+ return TRUE;
+}
+
+/**
+ * Read the RX IRQ status register, the rx status register and the rx packet
+ */
+bool_t cyrf6936_read_rx_irq_status_packet(struct Cyrf6936 *cyrf) {
+ /* Check if the cyrf6936 isn't busy */
+ if(cyrf->status != CYRF6936_IDLE)
+ return FALSE;
+
+ // Set the status
+ cyrf->status = CYRF6936_RX_IRQ_STATUS_PACKET;
+
+ /* Try to read the RX status */
+ cyrf->buffer_idx = 0;
+ cyrf6936_read_register(cyrf, CYRF_RX_IRQ_STATUS);
+
+ return TRUE;
+}
+
+/**
+ * Send a packet with a certain length
+ */
+bool_t cyrf6936_send(struct Cyrf6936 *cyrf, const uint8_t data[], const uint8_t length) {
+ int i;
+
+ /* Check if the cyrf6936 isn't busy */
+ if(cyrf->status != CYRF6936_IDLE)
+ return FALSE;
+
+ // Set the status
+ cyrf->status = CYRF6936_SEND;
+
+ // Copy the length and the data
+ cyrf->buffer[0] = length;
+ for(i = 0; i < length; i++)
+ cyrf->buffer[i+1] = data[i];
+
+ /* Try to set the packet length */
+ cyrf->buffer_idx = 0;
+ cyrf6936_write_register(cyrf, CYRF_TX_LENGTH, cyrf->buffer[0]);
+
+ return TRUE;
+}
diff --git a/sw/airborne/peripherals/cyrf6936.h b/sw/airborne/peripherals/cyrf6936.h
new file mode 100644
index 0000000000..c384a9878b
--- /dev/null
+++ b/sw/airborne/peripherals/cyrf6936.h
@@ -0,0 +1,77 @@
+/*
+ * Copyright (C) 2013 Freek van Tienen
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file peripherals/cyrf6936.h
+ * Driver for the cyrf6936 2.4GHz radio chip
+ */
+
+#ifndef CYRF6936_H
+#define CYRF6936_H
+
+#include "cyrf6936_regs.h"
+#include "mcu_periph/spi.h"
+
+#define CYRF6936_MAX_BUFFER 80 /**< The max buffer size in the cyrf6936 structure */
+
+/* The different statuses the cyrf6936 chip can be in */
+enum Cyrf6936Status {
+ CYRF6936_UNINIT, /**< The chip isn't initialized */
+ CYRF6936_IDLE, /**< The chip is idle and can be used */
+ CYRF6936_GET_MFG_ID, /**< The chip is busy with getting the manufacturer ID */
+ CYRF6936_MULTIWRITE, /**< The chip is writing multiple registers */
+ CYRF6936_DATA_CODE, /**< The chip is writing a data code */
+ CYRF6936_CHAN_SOP_DATA_CRC, /**< The chip is setting the channel, SOP code, DATA code and the CRC seed */
+ CYRF6936_RX_IRQ_STATUS_PACKET, /**< The chip is getting the receive irq status, receive status and the receive packet */
+ CYRF6936_SEND /**< The chip is busy sending a packet */
+};
+
+/* The structure for the cyrf6936 chip that handles all the buffers and requests */
+struct Cyrf6936 {
+ struct spi_periph *spi_p; /**< The SPI peripheral for the connection */
+ struct spi_transaction spi_t; /**< The SPI transaction used for the writing and reading of registers */
+ volatile enum Cyrf6936Status status; /**< The status of the CYRF6936 chip */
+ uint8_t input_buf[17]; /**< The input buffer for the SPI transaction */
+ uint8_t output_buf[17]; /**< The output buffer for the SPI transaction */
+
+ uint8_t buffer[CYRF6936_MAX_BUFFER]; /**< The buffer used to write/read multiple registers */
+ uint8_t buffer_length; /**< The length of the buffer used for MULTIWRITE */
+ uint8_t buffer_idx; /**< The index of the buffer used for MULTIWRITE and used as sub-status for other statuses */
+
+ bool_t has_irq; /**< When the CYRF6936 is done reading the irq */
+ uint8_t mfg_id[6]; /**< The manufacturer id of the CYRF6936 chip */
+ uint8_t tx_irq_status; /**< The last send interrupt status */
+ uint8_t rx_irq_status; /**< The last receive interrupt status */
+ uint8_t rx_status; /**< The last receive status */
+ uint8_t rx_count; /**< The length of the received packet */
+ uint8_t rx_packet[16]; /**< The last received packet */
+};
+
+extern void cyrf6936_init(struct Cyrf6936 *cyrf, struct spi_periph *spi_p, const uint8_t slave_idx, const uint32_t rst_port, const uint16_t rst_pin);
+void cyrf6936_event(struct Cyrf6936 *cyrf);
+
+bool_t cyrf6936_write(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t data);
+bool_t cyrf6936_multi_write(struct Cyrf6936 *cyrf, const uint8_t data[][2], const uint8_t length);
+bool_t cyrf6936_write_chan_sop_data_crc(struct Cyrf6936 *cyrf, const uint8_t chan, const uint8_t sop_code[], const uint8_t data_code[], const uint16_t crc_seed);
+bool_t cyrf6936_read_rx_irq_status_packet(struct Cyrf6936 *cyrf);
+bool_t cyrf6936_send(struct Cyrf6936 *cyrf, const uint8_t data[], const uint8_t length);
+
+#endif /* CYRF6936_H */
diff --git a/sw/airborne/peripherals/cyrf6936_regs.h b/sw/airborne/peripherals/cyrf6936_regs.h
new file mode 100644
index 0000000000..c5926771a7
--- /dev/null
+++ b/sw/airborne/peripherals/cyrf6936_regs.h
@@ -0,0 +1,211 @@
+/*
+ * Copyright (C) 2013 Freek van Tienen
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file peripherals/cyrf6936_regs.h
+ * Register defines for the CYRF6936 2.4GHz radio chip
+ */
+
+#ifndef CYRF6936_REGS_H
+#define CYRF6936_REGS_H
+
+/* The SPI interface defines */
+enum {
+ CYRF_CHANNEL = 0x00,
+ CYRF_TX_LENGTH = 0x01,
+ CYRF_TX_CTRL = 0x02,
+ CYRF_TX_CFG = 0x03,
+ CYRF_TX_IRQ_STATUS = 0x04,
+ CYRF_RX_CTRL = 0x05,
+ CYRF_RX_CFG = 0x06,
+ CYRF_RX_IRQ_STATUS = 0x07,
+ CYRF_RX_STATUS = 0x08,
+ CYRF_RX_COUNT = 0x09,
+ CYRF_RX_LENGTH = 0x0A,
+ CYRF_PWR_CTRL = 0x0B,
+ CYRF_XTAL_CTRL = 0x0C,
+ CYRF_IO_CFG = 0x0D,
+ CYRF_GPIO_CTRL = 0x0E,
+ CYRF_XACT_CFG = 0x0F,
+ CYRF_FRAMING_CFG = 0x10,
+ CYRF_DATA32_THOLD = 0x11,
+ CYRF_DATA64_THOLD = 0x12,
+ CYRF_RSSI = 0x13,
+ CYRF_EOP_CTRL = 0x14,
+ CYRF_CRC_SEED_LSB = 0x15,
+ CYRF_CRC_SEED_MSB = 0x16,
+ CYRF_TX_CRC_LSB = 0x17,
+ CYRF_TX_CRC_MSB = 0x18,
+ CYRF_RX_CRC_LSB = 0x19,
+ CYRF_RX_CRC_MSB = 0x1A,
+ CYRF_TX_OFFSET_LSB = 0x1B,
+ CYRF_TX_OFFSET_MSB = 0x1C,
+ CYRF_MODE_OVERRIDE = 0x1D,
+ CYRF_RX_OVERRIDE = 0x1E,
+ CYRF_TX_OVERRIDE = 0x1F,
+ CYRF_TX_BUFFER = 0x20,
+ CYRF_RX_BUFFER = 0x21,
+ CYRF_SOP_CODE = 0x22,
+ CYRF_DATA_CODE = 0x23,
+ CYRF_PREAMBLE = 0x24,
+ CYRF_MFG_ID = 0x25,
+ CYRF_XTAL_CFG = 0x26,
+ CYRF_CLK_OFFSET = 0x27,
+ CYRF_CLK_EN = 0x28,
+ CYRF_RX_ABORT = 0x29,
+ CYRF_AUTO_CAL_TIME = 0x32,
+ CYRF_AUTO_CAL_OFFSET = 0x35,
+ CYRF_ANALOG_CTRL = 0x39,
+};
+#define CYRF_DIR (1<<7) /**< Bit for enabling writing */
+
+// CYRF_MODE_OVERRIDE
+#define CYRF_RST (1<<0)
+
+// CYRF_CLK_EN
+#define CYRF_RXF (1<<1)
+
+// CYRF_XACT_CFG
+enum {
+ CYRF_MODE_SLEEP = (0x0 <<2),
+ CYRF_MODE_IDLE = (0x1 <<2),
+ CYRF_MODE_SYNTH_TX = (0x2 <<2),
+ CYRF_MODE_SYNTH_RX = (0x3 <<2),
+ CYRF_MODE_RX = (0x4 <<2),
+};
+#define CYRF_FRC_END (1<<5)
+#define CYRF_ACK_EN (1<<7)
+
+// CYRF_IO_CFG
+#define CYRF_IRQ_GPIO (1<<0)
+#define CYRF_SPI_3PIN (1<<1)
+#define CYRF_PACTL_GPIO (1<<2)
+#define CYRF_PACTL_OD (1<<3)
+#define CYRF_XOUT_OD (1<<4)
+#define CYRF_MISO_OD (1<<5)
+#define CYRF_IRQ_POL (1<<6)
+#define CYRF_IRQ_OD (1<<7)
+
+// CYRF_FRAMING_CFG
+#define CYRF_LEN_EN (1<<5)
+#define CYRF_SOP_LEN (1<<6)
+#define CYRF_SOP_EN (1<<7)
+
+// CYRF_RX_STATUS
+enum {
+ CYRF_RX_DATA_MODE_GFSK = 0x00,
+ CYRF_RX_DATA_MODE_8DR = 0x01,
+ CYRF_RX_DATA_MODE_DDR = 0x10,
+ CYRF_RX_DATA_MODE_NV = 0x11,
+};
+#define CYRF_RX_CODE (1<<2)
+#define CYRF_BAD_CRC (1<<3)
+#define CYRF_CRC0 (1<<4)
+#define CYRF_EOP_ERR (1<<5)
+#define CYRF_PKT_ERR (1<<6)
+#define CYRF_RX_ACK (1<<7)
+
+// CYRF_TX_IRQ_STATUS
+#define CYRF_TXE_IRQ (1<<0)
+#define CYRF_TXC_IRQ (1<<1)
+#define CYRF_TXBERR_IRQ (1<<2)
+#define CYRF_TXB0_IRQ (1<<3)
+#define CYRF_TXB8_IRQ (1<<4)
+#define CYRF_TXB15_IRQ (1<<5)
+#define CYRF_LV_IRQ (1<<6)
+#define CYRF_OS_IRQ (1<<7)
+
+// CYRF_RX_IRQ_STATUS
+#define CYRF_RXE_IRQ (1<<0)
+#define CYRF_RXC_IRQ (1<<1)
+#define CYRF_RXBERR_IRQ (1<<2)
+#define CYRF_RXB1_IRQ (1<<3)
+#define CYRF_RXB8_IRQ (1<<4)
+#define CYRF_RXB16_IRQ (1<<5)
+#define CYRF_SOPDET_IRQ (1<<6)
+#define CYRF_RXOW_IRQ (1<<7)
+
+// CYRF_TX_CTRL
+#define CYRF_TXE_IRQEN (1<<0)
+#define CYRF_TXC_IRQEN (1<<1)
+#define CYRF_TXBERR_IRQEN (1<<2)
+#define CYRF_TXB0_IRQEN (1<<3)
+#define CYRF_TXB8_IRQEN (1<<4)
+#define CYRF_TXB15_IRQEN (1<<5)
+#define CYRF_TX_CLR (1<<6)
+#define CYRF_TX_GO (1<<7)
+
+// CYRF_RX_CTRL
+#define CYRF_RXE_IRQEN (1<<0)
+#define CYRF_RXC_IRQEN (1<<1)
+#define CYRF_RXBERR_IRQEN (1<<2)
+#define CYRF_RXB1_IRQEN (1<<3)
+#define CYRF_RXB8_IRQEN (1<<4)
+#define CYRF_RXB16_IRQEN (1<<5)
+#define CYRF_RSVD (1<<6)
+#define CYRF_RX_GO (1<<7)
+
+// CYRF_RX_OVERRIDE
+#define CYRF_ACE (1<<1)
+#define CYRF_DIS_RXCRC (1<<2)
+#define CYRF_DIS_CRC0 (1<<3)
+#define CYRF_FRC_RXDR (1<<4)
+#define CYRF_MAN_RXACK (1<<5)
+#define CYRF_RXTX_DLY (1<<6)
+#define CYRF_ACK_RX (1<<7)
+
+// CYRF_TX_OVERRIDE
+#define CYRF_TX_INV (1<<0)
+#define CYRF_DIS_TXCRC (1<<2)
+#define CYRF_OVRD_ACK (1<<3)
+#define CYRF_MAN_TXACK (1<<4)
+#define CYRF_FRC_PRE (1<<6)
+#define CYRF_ACK_TX (1<<7)
+
+// CYRF_RX_CFG
+#define CYRF_VLD_EN (1<<0)
+#define CYRF_RXOW_EN (1<<1)
+#define CYRF_FAST_TURN_EN (1<<3)
+#define CYRF_HILO (1<<4)
+#define CYRF_ATT (1<<5)
+#define CYRF_LNA (1<<6)
+#define CYRF_AGC_EN (1<<7)
+
+// CYRF_TX_CFG
+enum {
+ CYRF_PA_M35 = 0x0,
+ CYRF_PA_M30 = 0x1,
+ CYRF_PA_M24 = 0x2,
+ CYRF_PA_M18 = 0x3,
+ CYRF_PA_M13 = 0x4,
+ CYRF_PA_M5 = 0x5,
+ CYRF_PA_0 = 0x6,
+ CYRF_PA_4 = 0x7,
+};
+enum {
+ CYRF_DATA_MODE_GFSK = (0x0 <<3),
+ CYRF_DATA_MODE_8DR = (0x1 <<3),
+ CYRF_DATA_MODE_DDR = (0x2 <<3),
+ CYRF_DATA_MODE_SDR = (0x3 <<3),
+};
+#define CYRF_DATA_CODE_LENGTH (1<<5)
+
+#endif // CYRF6936_REGS_H
diff --git a/sw/airborne/peripherals/l3g4200.c b/sw/airborne/peripherals/l3g4200.c
index 757f8ea8e3..64d82302c7 100644
--- a/sw/airborne/peripherals/l3g4200.c
+++ b/sw/airborne/peripherals/l3g4200.c
@@ -1,5 +1,4 @@
/*
- *
* Copyright (C) 2011 Gautier Hattenberger
* 2013 Felix Ruess
* 2013 Eduardo Lavratti
@@ -33,6 +32,7 @@
void l3g4200_set_default_config(struct L3g4200Config *c) {
c->ctrl_reg1 = L3G4200_DEFAULT_CTRL_REG1;
+ c->ctrl_reg4 = L3G4200_DEFAULT_CTRL_REG4;
c->ctrl_reg5 = L3G4200_DEFAULT_CTRL_REG5;
}
@@ -71,6 +71,10 @@ static void l3g4200_send_config(struct L3g4200 *l3g)
l3g4200_i2c_tx_reg(l3g, L3G4200_REG_CTRL_REG1, l3g->config.ctrl_reg1);
l3g->init_status++;
break;
+ case L3G_CONF_REG4:
+ l3g4200_i2c_tx_reg(l3g, L3G4200_REG_CTRL_REG4, l3g->config.ctrl_reg4);
+ l3g->init_status++;
+ break;
case L3G_CONF_REG5:
l3g4200_i2c_tx_reg(l3g, L3G4200_REG_CTRL_REG5, l3g->config.ctrl_reg5);
l3g->init_status++;
@@ -99,12 +103,12 @@ void l3g4200_start_configure(struct L3g4200 *l3g)
void l3g4200_read(struct L3g4200 *l3g)
{
if (l3g->initialized && l3g->i2c_trans.status == I2CTransDone) {
- l3g->i2c_trans.buf[0] = L3G4200_REG_STATUS_REG;
- i2c_transceive(l3g->i2c_p, &(l3g->i2c_trans), l3g->i2c_trans.slave_addr, 1, 9);
+ l3g->i2c_trans.buf[0] = 0x80 | L3G4200_REG_STATUS_REG;
+ i2c_transceive(l3g->i2c_p, &(l3g->i2c_trans), l3g->i2c_trans.slave_addr, 1, 7);
}
}
-#define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx]<<8) | _buf[_idx+1]))
+#define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx+1]<<8) | _buf[_idx]))
void l3g4200_event(struct L3g4200 *l3g)
{
@@ -114,11 +118,11 @@ void l3g4200_event(struct L3g4200 *l3g)
}
else if (l3g->i2c_trans.status == I2CTransSuccess) {
// Successfull reading and new data available
- if (l3g->i2c_trans.buf[0] & 0x01) { // ver oque é o sinal antes do &
+ if (l3g->i2c_trans.buf[0] & 0x08) {
// New data available
- l3g->data.rates.p = Int16FromBuf(l3g->i2c_trans.buf,3);
- l3g->data.rates.q = Int16FromBuf(l3g->i2c_trans.buf,5);
- l3g->data.rates.r = Int16FromBuf(l3g->i2c_trans.buf,7);
+ l3g->data.rates.p = Int16FromBuf(l3g->i2c_trans.buf,1);
+ l3g->data.rates.q = Int16FromBuf(l3g->i2c_trans.buf,3);
+ l3g->data.rates.r = Int16FromBuf(l3g->i2c_trans.buf,5);
l3g->data_available = TRUE;
}
l3g->i2c_trans.status = I2CTransDone;
diff --git a/sw/airborne/peripherals/l3g4200.h b/sw/airborne/peripherals/l3g4200.h
index 16b572ee52..ccfa6d25a8 100644
--- a/sw/airborne/peripherals/l3g4200.h
+++ b/sw/airborne/peripherals/l3g4200.h
@@ -19,7 +19,6 @@
* 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.
- *
*/
/**
@@ -38,25 +37,29 @@
#include "peripherals/l3g4200_regs.h"
-/// Default Output rate 800hz
-#define L3G4200_DEFAULT_DR L3G4200_DR_800Hz
-/// Default digital lowpass filter 35hz
-#define L3G4200_DEFAULT_DLPF L3G4200_DLPF_2
-
+// Default Output rate 100hz
+#define L3G4200_DEFAULT_DR L3G4200_DR_100Hz
+// Default digital lowpass filter 25hz
+#define L3G4200_DEFAULT_DLPF L3G4200_DLPF_1
+// Default Scale
+#define L3G4200_DEFAULT_SCALE L3G4200_SCALE_2000
/* Default conf */
-#define L3G4200_DEFAULT_CTRL_REG1 0x8f // 400hz ODR, 20hz filter, run!
-#define L3G4200_DEFAULT_CTRL_REG5 0x02 // low pass filter enable
+#define L3G4200_DEFAULT_CTRL_REG1 ((L3G4200_DEFAULT_DR<<6) | (L3G4200_DEFAULT_DLPF<<4) | 0xf);
+#define L3G4200_DEFAULT_CTRL_REG4 (L3G4200_DEFAULT_SCALE<<4) | 0x00; // 2000deg = 0x30
+#define L3G4200_DEFAULT_CTRL_REG5 0x00 // first low pass filter enable
struct L3g4200Config {
- uint8_t ctrl_reg1; ///<
- uint8_t ctrl_reg5; ///<
+ uint8_t ctrl_reg1;
+ uint8_t ctrl_reg4;
+ uint8_t ctrl_reg5;
};
/** config status states */
enum L3g4200ConfStatus {
L3G_CONF_UNINIT,
L3G_CONF_REG1,
+ L3G_CONF_REG4,
L3G_CONF_REG5,
L3G_CONF_DONE
};
@@ -75,7 +78,7 @@ struct L3g4200 {
};
// Functions
-extern void l3g4200_init(struct L3g4200 *itg, struct i2c_periph *i2c_p, uint8_t i2c_address);
+extern void l3g4200_init(struct L3g4200 *l3g, struct i2c_periph *i2c_p, uint8_t i2c_address);
extern void l3g4200_set_default_config(struct L3g4200Config *conf);
extern void l3g4200_start_configure(struct L3g4200 *l3g);
extern void l3g4200_read(struct L3g4200 *l3g);
diff --git a/sw/airborne/peripherals/l3g4200_regs.h b/sw/airborne/peripherals/l3g4200_regs.h
index 46f8486d20..57082f11bb 100644
--- a/sw/airborne/peripherals/l3g4200_regs.h
+++ b/sw/airborne/peripherals/l3g4200_regs.h
@@ -70,6 +70,7 @@ enum L3g4200_DR {
L3G4200_DR_400Hz = 0x2,
L3G4200_DR_800Hz = 0x3
};
+
/** Digital Low Pass Filter Options */
enum L3g4200_DLPF {
L3G4200_DLPF_1 = 0x0,
@@ -78,4 +79,10 @@ enum L3g4200_DLPF {
L3G4200_DLPF_4 = 0x3
};
+enum L3g4200_SCALE {
+ L3G4200_SCALE_250 = 0x0,
+ L3G4200_SCALE_500 = 0x1,
+ L3G4200_SCALE_2000 = 0x2,
+};
+
#endif /* L3G4200_REGS_H */
diff --git a/sw/airborne/peripherals/ms5611.c b/sw/airborne/peripherals/ms5611.c
new file mode 100644
index 0000000000..9956d6583b
--- /dev/null
+++ b/sw/airborne/peripherals/ms5611.c
@@ -0,0 +1,91 @@
+/*
+ * Copyright (C) 2011 Martin Mueller
+ * Copyright (C) 2013 Felix Ruess
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file peripherals/ms5611.c
+ *
+ * MS5611 barometer driver common functions (I2C and SPI).
+ */
+
+#include "peripherals/ms5611.h"
+#include "std.h"
+
+/**
+ * Check if CRC of PROM data is OK.
+ * @return TRUE if OK, FALSE otherwise
+ */
+bool_t ms5611_prom_crc_ok(uint16_t* prom) {
+ int32_t i, j;
+ uint32_t res = 0;
+ uint8_t crc = prom[7] & 0xF;
+ prom[7] &= 0xFF00;
+ for (i = 0; i < 16; i++) {
+ if (i & 1)
+ res ^= ((prom[i>>1]) & 0x00FF);
+ else
+ res ^= (prom[i>>1]>>8);
+ for (j = 8; j > 0; j--) {
+ if (res & 0x8000)
+ res ^= 0x1800;
+ res <<= 1;
+ }
+ }
+ prom[7] |= crc;
+ if (crc == ((res >> 12) & 0xF))
+ return TRUE;
+ else
+ return FALSE;
+}
+
+/**
+ * Calculate temperature and compensated pressure.
+ */
+void ms5611_calc(struct Ms5611Data* ms) {
+ int64_t dt, tempms, off, sens, t2, off2, sens2;
+
+ /* difference between actual and ref temperature */
+ dt = ms->d2 - (int64_t)ms->c[5] * (1<<8);
+ /* actual temperature */
+ tempms = 2000 + ((int64_t)dt * ms->c[6]) / (1<<23);
+ /* offset at actual temperature */
+ off = ((int64_t)ms->c[2] * (1<<16)) + ((int64_t)ms->c[4] * dt) / (1<<7);
+ /* sensitivity at actual temperature */
+ sens = ((int64_t)ms->c[1] * (1<<15)) + ((int64_t)ms->c[3] * dt) / (1<<8);
+ /* second order temperature compensation */
+ if (tempms < 2000) {
+ t2 = (dt*dt) / (1<<31);
+ off2 = 5 * ((int64_t)(tempms-2000)*(tempms-2000)) / (1<<1);
+ sens2 = 5 * ((int64_t)(tempms-2000)*(tempms-2000)) / (1<<2);
+ if (tempms < -1500) {
+ off2 = off2 + 7 * (int64_t)(tempms+1500)*(tempms+1500);
+ sens2 = sens2 + 11 * ((int64_t)(tempms+1500)*(tempms+1500)) / (1<<1);
+ }
+ tempms = tempms - t2;
+ off = off - off2;
+ sens = sens - sens2;
+ }
+
+ /* temperature in deg Celsius with 0.01 degC resolultion */
+ ms->temperature = (int32_t)tempms;
+ /* temperature compensated pressure in Pascal (0.01mbar) */
+ ms->pressure = (uint32_t)((((int64_t)ms->d1 * sens) / (1<<21) - off) / (1<<15));
+}
diff --git a/sw/airborne/peripherals/ms5611.h b/sw/airborne/peripherals/ms5611.h
index ad3054e776..85958d5ac5 100644
--- a/sw/airborne/peripherals/ms5611.h
+++ b/sw/airborne/peripherals/ms5611.h
@@ -1,6 +1,5 @@
/*
- *
- * Copyright (C) 2012 Piotr Esden-Tempski
+ * Copyright (C) 2013 Felix Ruess
*
* This file is part of paparazzi.
*
@@ -20,82 +19,44 @@
* Boston, MA 02111-1307, USA.
*/
-/* Register definition for MS5611
+/**
+ * @file peripherals/ms5611.h
+ *
+ * MS5611 barometer driver common interface (I2C and SPI).
*/
#ifndef MS5611_H
#define MS5611_H
-/* default i2c address
- * when CSB is set to GND addr is 0xEE
- * when CSB is set to VCC addr is 0xEC
- *
- * Note: Aspirin 2.1 has CSB bound to GND.
- */
-#define MS5611_SLAVE_ADDR 0xEE
+#include "std.h"
-/* FIXME: For backwards compatibility with Aspirin driver (it doesnt talk to baro either) */
-#define MS5611_ADDR0 0x77
-#define MS5611_ADDR1 0x76
+/* Include address and register definition */
+#include "peripherals/ms5611_regs.h"
-/* SPI SLAVE3 is on pin PC13
- * Aspirin 2.2 has ms5611 on SPI bus
- */
-#ifndef MS5611_SLAVE_DEV
-#define MS5611_SLAVE_DEV SPI_SLAVE3
-#endif
-/* Number of 16bit calibration coefficients */
-#define PROM_NB 8
-
-/* OSR definitions */
-#define MS5611_OSR256 0x02
-#define MS5611_OSR512 0x02
-#define MS5611_OSR1024 0x04
-#define MS5611_OSR2048 0x06
-#define MS5611_OSR4096 0x08
-
-/* D1 Register defines */
-#define MS5611_REG_D1R 0x40 // Request D1 (pressure) conversion
-#define MS5611_REG_D1(_osr) (MS5611_REG_D1R | _osr)
-#define MS5611_REG_D1OSR256 MS5611_REG_D1(MS5611_ORS256)
-#define MS5611_REG_D1OSR512 MS5611_REG_D1(MS5611_OSR512)
-#define MS5611_REG_D1OSR1024 MS5611_REG_D1(MS5611_OSR1024)
-#define MS5611_REG_D1OSR2048 MS5611_REG_D1(MS5611_OSR2048)
-#define MS5611_REG_D1OSR4096 MS5611_REG_D1(MS5611_OSR4096)
-
-/* D2 register defines */
-#define MS5611_REG_D2R 0x50 // Request D2 (temperature) conversion
-#define MS5611_REG_D2(_osr) (MS5611_REG_D2R | _osr)
-#define MS5611_REG_D2OSR256 MS5611_REG_D2(MS5611_ORS256)
-#define MS5611_REG_D2OSR512 MS5611_REG_D2(MS5611_OSR512)
-#define MS5611_REG_D2OSR1024 MS5611_REG_D2(MS5611_OSR1024)
-#define MS5611_REG_D2OSR2048 MS5611_REG_D2(MS5611_OSR2048)
-#define MS5611_REG_D2OSR4096 MS5611_REG_D2(MS5611_OSR4096)
-
-/* Commands */
-#define MS5611_ADC_READ 0x00 // Read converted value
-#define MS5611_SOFT_RESET 0x1E // Reset command
-#define MS5611_PROM_READ 0xA0 // Start reading PROM
-#define MS5611_START_CONV_D1 MS5611_REG_D1OSR4096 /* we use OSR=4096 for maximum resolution */
-#define MS5611_START_CONV_D2 MS5611_REG_D2OSR4096 /* we use OSR=4096 for maximum resolution */
-
-/* FIXME: backwards compatibility with Aspirin driver */
-#define MS5611_REG_RESET MS5611_SOFT_RESET
-#define MS5611_REG_ADCREAD MS5611_ADC_READ
-
-enum ms5611_stat{
- MS5611_UNINIT,
- MS5611_RESET,
- MS5611_RESET_OK,
- MS5611_PROM,
- MS5611_IDLE,
- MS5611_CONV_D1,
- MS5611_CONV_D1_OK,
- MS5611_ADC_D1,
- MS5611_CONV_D2,
- MS5611_CONV_D2_OK,
- MS5611_ADC_D2
+enum Ms5611Status {
+ MS5611_STATUS_UNINIT,
+ MS5611_STATUS_RESET,
+ MS5611_STATUS_RESET_OK,
+ MS5611_STATUS_PROM,
+ MS5611_STATUS_IDLE,
+ MS5611_STATUS_CONV_D1,
+ MS5611_STATUS_CONV_D1_OK,
+ MS5611_STATUS_ADC_D1,
+ MS5611_STATUS_CONV_D2,
+ MS5611_STATUS_CONV_D2_OK,
+ MS5611_STATUS_ADC_D2
};
+struct Ms5611Data {
+ uint32_t pressure; ///< pressure in Pascal (0.01mbar)
+ int32_t temperature; ///< temperature with 0.01 degrees Celsius resolution
+ uint16_t c[PROM_NB];
+ uint32_t d1;
+ uint32_t d2;
+};
+
+extern bool_t ms5611_prom_crc_ok(uint16_t* prom);
+extern void ms5611_calc(struct Ms5611Data* ms);
+
#endif /* MS5611_H */
diff --git a/sw/airborne/peripherals/ms5611_i2c.c b/sw/airborne/peripherals/ms5611_i2c.c
new file mode 100644
index 0000000000..1ec14b4611
--- /dev/null
+++ b/sw/airborne/peripherals/ms5611_i2c.c
@@ -0,0 +1,205 @@
+/*
+ * Copyright (C) 2011 Martin Mueller
+ * Copyright (C) 2013 Felix Ruess
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file peripherals/ms5611_i2c.c
+ * Measurement Specialties (Intersema) MS5611-01BA pressure/temperature sensor interface for I2C.
+ *
+ */
+
+
+#include "peripherals/ms5611_i2c.h"
+
+
+void ms5611_i2c_init(struct Ms5611_I2c *ms, struct i2c_periph *i2c_p, uint8_t addr)
+{
+ /* set i2c_peripheral */
+ ms->i2c_p = i2c_p;
+
+ /* slave address */
+ ms->i2c_trans.slave_addr = addr;
+ /* set initial status: Success or Done */
+ ms->i2c_trans.status = I2CTransDone;
+
+ ms->data_available = FALSE;
+ ms->initialized = FALSE;
+ ms->status = MS5611_STATUS_UNINIT;
+ ms->prom_cnt = 0;
+}
+
+void ms5611_i2c_start_configure(struct Ms5611_I2c *ms)
+{
+ if (ms->status == MS5611_STATUS_UNINIT) {
+ ms->initialized = FALSE;
+ ms->prom_cnt = 0;
+ ms->i2c_trans.buf[0] = MS5611_SOFT_RESET;
+ i2c_transmit(ms->i2c_p, &(ms->i2c_trans), ms->i2c_trans.slave_addr, 1);
+ ms->status = MS5611_STATUS_RESET;
+ }
+}
+
+void ms5611_i2c_start_conversion(struct Ms5611_I2c *ms)
+{
+ if (ms->status == MS5611_STATUS_IDLE &&
+ ms->i2c_trans.status == I2CTransDone) {
+ /* start D1 conversion */
+ ms->i2c_trans.buf[0] = MS5611_START_CONV_D1;
+ i2c_transmit(ms->i2c_p, &(ms->i2c_trans), ms->i2c_trans.slave_addr, 1);
+ ms->status = MS5611_STATUS_CONV_D1;
+ }
+}
+
+/**
+ * Periodic function to ensure proper delay after triggering reset or conversion.
+ * Should run at 100Hz max.
+ * Typical conversion time is 8.22ms at max resolution.
+ */
+void ms5611_i2c_periodic_check(struct Ms5611_I2c *ms)
+{
+ switch (ms->status) {
+ case MS5611_STATUS_RESET:
+ ms->status = MS5611_STATUS_RESET_OK;
+ break;
+ case MS5611_STATUS_RESET_OK:
+ if (ms->i2c_trans.status == I2CTransDone) {
+ /* start getting prom data */
+ ms->i2c_trans.buf[0] = MS5611_PROM_READ | (ms->prom_cnt << 1);
+ i2c_transceive(ms->i2c_p, &(ms->i2c_trans), ms->i2c_trans.slave_addr, 1, 2);
+ ms->status = MS5611_STATUS_PROM;
+ }
+ break;
+ case MS5611_STATUS_CONV_D1:
+ ms->status = MS5611_STATUS_CONV_D1_OK;
+ break;
+ case MS5611_STATUS_CONV_D1_OK:
+ if (ms->i2c_trans.status == I2CTransDone) {
+ /* read D1 adc */
+ ms->i2c_trans.buf[0] = MS5611_ADC_READ;
+ i2c_transceive(ms->i2c_p, &(ms->i2c_trans), ms->i2c_trans.slave_addr, 1, 3);
+ ms->status = MS5611_STATUS_ADC_D1;
+ }
+ break;
+ case MS5611_STATUS_CONV_D2:
+ ms->status = MS5611_STATUS_CONV_D2_OK;
+ break;
+ case MS5611_STATUS_CONV_D2_OK:
+ if (ms->i2c_trans.status == I2CTransDone) {
+ /* read D2 adc */
+ ms->i2c_trans.buf[0] = MS5611_ADC_READ;
+ i2c_transceive(ms->i2c_p, &(ms->i2c_trans), ms->i2c_trans.slave_addr, 1, 3);
+ ms->status = MS5611_STATUS_ADC_D2;
+ }
+ break;
+ default:
+ break;
+ }
+}
+
+void ms5611_i2c_event(struct Ms5611_I2c *ms) {
+ if (ms->initialized) {
+ if (ms->i2c_trans.status == I2CTransFailed) {
+ ms->status = MS5611_STATUS_IDLE;
+ ms->i2c_trans.status = I2CTransDone;
+ }
+ else if (ms->i2c_trans.status == I2CTransSuccess) {
+ // Successfull reading
+ switch (ms->status) {
+
+ case MS5611_STATUS_ADC_D1:
+ /* read D1 (pressure) */
+ ms->data.d1 = (ms->i2c_trans.buf[0] << 16) |
+ (ms->i2c_trans.buf[1] << 8) |
+ ms->i2c_trans.buf[2];
+ if (ms->data.d1 == 0) {
+ /* if value is zero, it was read to soon and is invalid, back to idle */
+ ms->status = MS5611_STATUS_IDLE;
+ }
+ else {
+ /* start D2 conversion */
+ ms->i2c_trans.buf[0] = MS5611_START_CONV_D2;
+ i2c_transmit(ms->i2c_p, &(ms->i2c_trans), ms->i2c_trans.slave_addr, 1);
+ ms->status = MS5611_STATUS_CONV_D2;
+ }
+ break;
+
+ case MS5611_STATUS_ADC_D2:
+ /* read D2 (temperature) */
+ ms->data.d2 = (ms->i2c_trans.buf[0] << 16) |
+ (ms->i2c_trans.buf[1] << 8) |
+ ms->i2c_trans.buf[2];
+ if (ms->data.d2 == 0) {
+ /* if value is zero, it was read to soon and is invalid, back to idle */
+ ms->status = MS5611_STATUS_IDLE;
+ }
+ else {
+ /* calculate temp and pressure from measurements */
+ ms5611_calc(&(ms->data));
+ ms->status = MS5611_STATUS_IDLE;
+ ms->data_available = TRUE;
+ }
+ break;
+
+ default:
+ break;
+ }
+ ms->i2c_trans.status = I2CTransDone;
+ }
+ }
+ else if (ms->status != MS5611_STATUS_UNINIT) { // Configuring but not yet initialized
+ switch (ms->i2c_trans.status) {
+
+ case I2CTransFailed:
+ /* try again */
+ ms->status = MS5611_STATUS_UNINIT;
+ ms->i2c_trans.status = I2CTransDone;
+ break;
+
+ case I2CTransSuccess:
+ if (ms->status == MS5611_STATUS_PROM) {
+ /* read prom data */
+ ms->data.c[ms->prom_cnt++] = (ms->i2c_trans.buf[0] << 8) |
+ ms->i2c_trans.buf[1];
+ if (ms->prom_cnt < PROM_NB) {
+ /* get next prom data */
+ ms->i2c_trans.buf[0] = MS5611_PROM_READ | (ms->prom_cnt << 1);
+ i2c_transceive(ms->i2c_p, &(ms->i2c_trans), ms->i2c_trans.slave_addr, 1, 2);
+ }
+ else {
+ /* done reading prom, check prom crc */
+ if (ms5611_prom_crc_ok(ms->data.c)) {
+ ms->initialized = TRUE;
+ ms->status = MS5611_STATUS_IDLE;
+ }
+ else {
+ /* checksum error, try again */
+ ms->status = MS5611_STATUS_UNINIT;
+ }
+ }
+ }
+ ms->i2c_trans.status = I2CTransDone;
+ break;
+
+ default:
+ break;
+ }
+ }
+}
diff --git a/sw/airborne/peripherals/ms5611_i2c.h b/sw/airborne/peripherals/ms5611_i2c.h
new file mode 100644
index 0000000000..149312ff36
--- /dev/null
+++ b/sw/airborne/peripherals/ms5611_i2c.h
@@ -0,0 +1,71 @@
+/*
+ * Copyright (C) 2013 Felix Ruess
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file peripherals/ms5611_i2c.h
+ *
+ * Measurement Specialties (Intersema) MS5611-01BA pressure/temperature sensor interface for I2C.
+ */
+
+#ifndef MS5611_I2C_H
+#define MS5611_I2C_H
+
+#include "mcu_periph/i2c.h"
+
+/* Include common MS5611 definitions */
+#include "peripherals/ms5611.h"
+
+struct Ms5611_I2c {
+ struct i2c_periph *i2c_p;
+ struct i2c_transaction i2c_trans;
+ enum Ms5611Status status;
+ bool_t initialized; ///< config done flag
+ volatile bool_t data_available; ///< data ready flag
+ struct Ms5611Data data;
+ int32_t prom_cnt; ///< number of bytes read from PROM
+};
+
+// Functions
+extern void ms5611_i2c_init(struct Ms5611_I2c* ms, struct i2c_periph* i2c_p, uint8_t addr);
+extern void ms5611_i2c_start_configure(struct Ms5611_I2c* ms);
+extern void ms5611_i2c_start_conversion(struct Ms5611_I2c* ms);
+extern void ms5611_i2c_periodic_check(struct Ms5611_I2c* ms);
+extern void ms5611_i2c_event(struct Ms5611_I2c* ms);
+
+/** convenience function to trigger new measurement.
+ * (or start configuration if not already initialized)
+ * Still need to regularly run ms5611_i2c_periodic_check to complete the measurement.
+ */
+static inline void ms5611_i2c_read(struct Ms5611_I2c* ms) {
+ if (ms->initialized)
+ ms5611_i2c_start_conversion(ms);
+ else
+ ms5611_i2c_start_configure(ms);
+}
+
+/// convenience function
+static inline void ms5611_i2c_periodic(struct Ms5611_I2c* ms) {
+ ms5611_i2c_read(ms);
+ ms5611_i2c_periodic_check(ms);
+}
+
+
+#endif /* MS5611_I2C_H */
diff --git a/sw/airborne/peripherals/ms5611_regs.h b/sw/airborne/peripherals/ms5611_regs.h
new file mode 100644
index 0000000000..6cd3eb1fe2
--- /dev/null
+++ b/sw/airborne/peripherals/ms5611_regs.h
@@ -0,0 +1,72 @@
+/*
+ * Copyright (C) 2012 Piotr Esden-Tempski
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file peripherals/ms5611_regs.h
+ * Register definitions for MS5611 barometer.
+ */
+
+#ifndef MS5611_REGS_H
+#define MS5611_REGS_H
+
+/** default i2c address
+ * when CSB is set to GND addr is 0xEE
+ * when CSB is set to VCC addr is 0xEC
+ */
+#define MS5611_I2C_SLAVE_ADDR 0xEE
+#define MS5611_I2C_SLAVE_ADDR_ALT 0xEC
+
+/* Number of 16bit calibration coefficients */
+#define PROM_NB 8
+
+/* OSR definitions */
+#define MS5611_OSR256 0x02
+#define MS5611_OSR512 0x02
+#define MS5611_OSR1024 0x04
+#define MS5611_OSR2048 0x06
+#define MS5611_OSR4096 0x08
+
+/* D1 Register defines */
+#define MS5611_REG_D1R 0x40 // Request D1 (pressure) conversion
+#define MS5611_REG_D1(_osr) (MS5611_REG_D1R | _osr)
+#define MS5611_REG_D1OSR256 MS5611_REG_D1(MS5611_ORS256)
+#define MS5611_REG_D1OSR512 MS5611_REG_D1(MS5611_OSR512)
+#define MS5611_REG_D1OSR1024 MS5611_REG_D1(MS5611_OSR1024)
+#define MS5611_REG_D1OSR2048 MS5611_REG_D1(MS5611_OSR2048)
+#define MS5611_REG_D1OSR4096 MS5611_REG_D1(MS5611_OSR4096)
+
+/* D2 register defines */
+#define MS5611_REG_D2R 0x50 // Request D2 (temperature) conversion
+#define MS5611_REG_D2(_osr) (MS5611_REG_D2R | _osr)
+#define MS5611_REG_D2OSR256 MS5611_REG_D2(MS5611_ORS256)
+#define MS5611_REG_D2OSR512 MS5611_REG_D2(MS5611_OSR512)
+#define MS5611_REG_D2OSR1024 MS5611_REG_D2(MS5611_OSR1024)
+#define MS5611_REG_D2OSR2048 MS5611_REG_D2(MS5611_OSR2048)
+#define MS5611_REG_D2OSR4096 MS5611_REG_D2(MS5611_OSR4096)
+
+/* Commands */
+#define MS5611_ADC_READ 0x00 // Read converted value
+#define MS5611_SOFT_RESET 0x1E // Reset command
+#define MS5611_PROM_READ 0xA0 // Start reading PROM
+#define MS5611_START_CONV_D1 MS5611_REG_D1OSR4096 /* we use OSR=4096 for maximum resolution */
+#define MS5611_START_CONV_D2 MS5611_REG_D2OSR4096 /* we use OSR=4096 for maximum resolution */
+
+#endif /* MS5611_REGS_H */
diff --git a/sw/airborne/peripherals/ms5611_spi.c b/sw/airborne/peripherals/ms5611_spi.c
new file mode 100644
index 0000000000..d8e9293185
--- /dev/null
+++ b/sw/airborne/peripherals/ms5611_spi.c
@@ -0,0 +1,219 @@
+/*
+ * Copyright (C) 2011 Martin Mueller
+ * Copyright (C) 2013 Felix Ruess
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file peripherals/ms5611_spi.c
+ * Measurement Specialties (Intersema) MS5611-01BA pressure/temperature sensor interface for SPI.
+ *
+ */
+
+
+#include "peripherals/ms5611_spi.h"
+
+
+void ms5611_spi_init(struct Ms5611_Spi *ms, struct spi_periph *spi_p, uint8_t slave_idx)
+{
+ /* set spi_peripheral */
+ ms->spi_p = spi_p;
+
+ /* configure spi transaction */
+ ms->spi_trans.cpol = SPICpolIdleHigh;
+ ms->spi_trans.cpha = SPICphaEdge2;
+ ms->spi_trans.dss = SPIDss8bit;
+ ms->spi_trans.bitorder = SPIMSBFirst;
+ ms->spi_trans.cdiv = SPIDiv64;
+
+ ms->spi_trans.select = SPISelectUnselect;
+ ms->spi_trans.slave_idx = slave_idx;
+ ms->spi_trans.output_length = 1;
+ ms->spi_trans.input_length = 4;
+ ms->spi_trans.before_cb = NULL;
+ ms->spi_trans.after_cb = NULL;
+ ms->spi_trans.input_buf = ms->rx_buf;
+ ms->spi_trans.output_buf = ms->tx_buf;
+
+ /* set initial status: Success or Done */
+ ms->spi_trans.status = SPITransDone;
+
+ ms->data_available = FALSE;
+ ms->initialized = FALSE;
+ ms->status = MS5611_STATUS_UNINIT;
+ ms->prom_cnt = 0;
+}
+
+void ms5611_spi_start_configure(struct Ms5611_Spi *ms)
+{
+ if (ms->status == MS5611_STATUS_UNINIT) {
+ ms->initialized = FALSE;
+ ms->prom_cnt = 0;
+ ms->tx_buf[0] = MS5611_SOFT_RESET;
+ spi_submit(ms->spi_p, &(ms->spi_trans));
+ ms->status = MS5611_STATUS_RESET;
+ }
+}
+
+void ms5611_spi_start_conversion(struct Ms5611_Spi *ms)
+{
+ if (ms->status == MS5611_STATUS_IDLE &&
+ ms->spi_trans.status == SPITransDone) {
+ /* start D1 conversion */
+ ms->tx_buf[0] = MS5611_START_CONV_D1;
+ spi_submit(ms->spi_p, &(ms->spi_trans));
+ ms->status = MS5611_STATUS_CONV_D1;
+ }
+}
+
+/**
+ * Periodic function to ensure proper delay after triggering reset or conversion.
+ * Should run at 100Hz max.
+ * Typical conversion time is 8.22ms at max resolution.
+ */
+void ms5611_spi_periodic_check(struct Ms5611_Spi *ms)
+{
+ switch (ms->status) {
+ case MS5611_STATUS_RESET:
+ ms->status = MS5611_STATUS_RESET_OK;
+ break;
+ case MS5611_STATUS_RESET_OK:
+ if (ms->spi_trans.status == SPITransDone) {
+ /* start getting prom data */
+ ms->tx_buf[0] = MS5611_PROM_READ | (ms->prom_cnt << 1);
+ spi_submit(ms->spi_p, &(ms->spi_trans));
+ ms->status = MS5611_STATUS_PROM;
+ }
+ break;
+ case MS5611_STATUS_CONV_D1:
+ ms->status = MS5611_STATUS_CONV_D1_OK;
+ break;
+ case MS5611_STATUS_CONV_D1_OK:
+ if (ms->spi_trans.status == SPITransDone) {
+ /* read D1 adc */
+ ms->tx_buf[0] = MS5611_ADC_READ;
+ spi_submit(ms->spi_p, &(ms->spi_trans));
+ ms->status = MS5611_STATUS_ADC_D1;
+ }
+ break;
+ case MS5611_STATUS_CONV_D2:
+ ms->status = MS5611_STATUS_CONV_D2_OK;
+ break;
+ case MS5611_STATUS_CONV_D2_OK:
+ if (ms->spi_trans.status == SPITransDone) {
+ /* read D2 adc */
+ ms->tx_buf[0] = MS5611_ADC_READ;
+ spi_submit(ms->spi_p, &(ms->spi_trans));
+ ms->status = MS5611_STATUS_ADC_D2;
+ }
+ break;
+ default:
+ break;
+ }
+}
+
+void ms5611_spi_event(struct Ms5611_Spi *ms) {
+ if (ms->initialized) {
+ if (ms->spi_trans.status == SPITransFailed) {
+ ms->status = MS5611_STATUS_IDLE;
+ ms->spi_trans.status = SPITransDone;
+ }
+ else if (ms->spi_trans.status == SPITransSuccess) {
+ // Successfull reading
+ switch (ms->status) {
+
+ case MS5611_STATUS_ADC_D1:
+ /* read D1 (pressure) */
+ ms->data.d1 = (ms->rx_buf[1] << 16) |
+ (ms->rx_buf[2] << 8) |
+ ms->rx_buf[3];
+ if (ms->data.d1 == 0) {
+ /* if value is zero, it was read to soon and is invalid, back to idle */
+ ms->status = MS5611_STATUS_IDLE;
+ }
+ else {
+ /* start D2 conversion */
+ ms->tx_buf[0] = MS5611_START_CONV_D2;
+ spi_submit(ms->spi_p, &(ms->spi_trans));
+ ms->status = MS5611_STATUS_CONV_D2;
+ }
+ break;
+
+ case MS5611_STATUS_ADC_D2:
+ /* read D2 (temperature) */
+ ms->data.d2 = (ms->rx_buf[1] << 16) |
+ (ms->rx_buf[2] << 8) |
+ ms->rx_buf[3];
+ if (ms->data.d2 == 0) {
+ /* if value is zero, it was read to soon and is invalid, back to idle */
+ ms->status = MS5611_STATUS_IDLE;
+ }
+ else {
+ /* calculate temp and pressure from measurements */
+ ms5611_calc(&(ms->data));
+ ms->status = MS5611_STATUS_IDLE;
+ ms->data_available = TRUE;
+ }
+ break;
+
+ default:
+ break;
+ }
+ ms->spi_trans.status = SPITransDone;
+ }
+ }
+ else if (ms->status != MS5611_STATUS_UNINIT) { // Configuring but not yet initialized
+ switch (ms->spi_trans.status) {
+
+ case SPITransFailed:
+ /* try again */
+ ms->status = MS5611_STATUS_UNINIT;
+ ms->spi_trans.status = SPITransDone;
+ break;
+
+ case SPITransSuccess:
+ if (ms->status == MS5611_STATUS_PROM) {
+ /* read prom data */
+ ms->data.c[ms->prom_cnt++] = (ms->rx_buf[1] << 8) |
+ ms->rx_buf[2];
+ if (ms->prom_cnt < PROM_NB) {
+ /* get next prom data */
+ ms->tx_buf[0] = MS5611_PROM_READ | (ms->prom_cnt << 1);
+ spi_submit(ms->spi_p, &(ms->spi_trans));
+ }
+ else {
+ /* done reading prom, check prom crc */
+ if (ms5611_prom_crc_ok(ms->data.c)) {
+ ms->initialized = TRUE;
+ ms->status = MS5611_STATUS_IDLE;
+ }
+ else {
+ /* checksum error, try again */
+ ms->status = MS5611_STATUS_UNINIT;
+ }
+ }
+ }
+ ms->spi_trans.status = SPITransDone;
+ break;
+
+ default:
+ break;
+ }
+ }
+}
diff --git a/sw/airborne/peripherals/ms5611_spi.h b/sw/airborne/peripherals/ms5611_spi.h
new file mode 100644
index 0000000000..59f8f6e8e8
--- /dev/null
+++ b/sw/airborne/peripherals/ms5611_spi.h
@@ -0,0 +1,73 @@
+/*
+ * Copyright (C) 2013 Felix Ruess
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file peripherals/ms5611_spi.h
+ *
+ * Measurement Specialties (Intersema) MS5611-01BA pressure/temperature sensor interface for SPI.
+ */
+
+#ifndef MS5611_SPI_H
+#define MS5611_SPI_H
+
+#include "mcu_periph/spi.h"
+
+/* Include common MS5611 definitions */
+#include "peripherals/ms5611.h"
+
+struct Ms5611_Spi {
+ struct spi_periph *spi_p;
+ struct spi_transaction spi_trans;
+ volatile uint8_t tx_buf[1];
+ volatile uint8_t rx_buf[4];
+ enum Ms5611Status status;
+ bool_t initialized; ///< config done flag
+ volatile bool_t data_available; ///< data ready flag
+ struct Ms5611Data data;
+ int32_t prom_cnt; ///< number of bytes read from PROM
+};
+
+// Functions
+extern void ms5611_spi_init(struct Ms5611_Spi* ms, struct spi_periph* spi_p, uint8_t addr);
+extern void ms5611_spi_start_configure(struct Ms5611_Spi* ms);
+extern void ms5611_spi_start_conversion(struct Ms5611_Spi* ms);
+extern void ms5611_spi_periodic_check(struct Ms5611_Spi* ms);
+extern void ms5611_spi_event(struct Ms5611_Spi* ms);
+
+/** convenience function to trigger new measurement.
+ * (or start configuration if not already initialized)
+ * Still need to regularly run ms5611_spi_periodic_check to complete the measurement.
+ */
+static inline void ms5611_spi_read(struct Ms5611_Spi* ms) {
+ if (ms->initialized)
+ ms5611_spi_start_conversion(ms);
+ else
+ ms5611_spi_start_configure(ms);
+}
+
+/// convenience function
+static inline void ms5611_spi_periodic(struct Ms5611_Spi* ms) {
+ ms5611_spi_read(ms);
+ ms5611_spi_periodic_check(ms);
+}
+
+
+#endif /* MS5611_SPI_H */
diff --git a/sw/airborne/state.h b/sw/airborne/state.h
index b64ac2d95c..ee1436f1c4 100644
--- a/sw/airborne/state.h
+++ b/sw/airborne/state.h
@@ -148,7 +148,7 @@ struct State {
/**
* Position in Latitude, Longitude and Altitude.
* Units lat,lon: radians*1e7
- * Units alt: centimeters above MSL
+ * Units alt: milimeters above reference ellipsoid
*/
struct LlaCoor_i lla_pos_i;
@@ -196,7 +196,7 @@ struct State {
/**
* Position in Latitude, Longitude and Altitude.
* Units lat,lon: radians
- * Units alt: meters above MSL
+ * Units alt: meters above reference ellipsoid
*/
struct LlaCoor_f lla_pos_f;
@@ -487,12 +487,12 @@ extern void stateCalcPositionLla_f(void);
/// Test if local coordinates are valid.
static inline bool_t stateIsLocalCoordinateValid(void) {
- return ((state.ned_initialized_i || state.utm_initialized_f) && (state.pos_status & ~(POS_LOCAL_COORD)));
+ return ((state.ned_initialized_i || state.utm_initialized_f) && (state.pos_status & (POS_LOCAL_COORD)));
}
/// Test if global coordinates are valid.
static inline bool_t stateIsGlobalCoordinateValid(void) {
- return ((state.pos_status & ~(POS_GLOBAL_COORD)) || stateIsLocalCoordinateValid());
+ return ((state.pos_status & (POS_GLOBAL_COORD)) || stateIsLocalCoordinateValid());
}
/************************ Set functions ****************************/
@@ -1156,7 +1156,7 @@ static inline void stateSetAirspeed_f(float* airspeed) {
SetBit(state.wind_air_status, AIRSPEED_F);
}
-/// Set angle of attack (float).
+/// Set angle of attack in radians (float).
static inline void stateSetAngleOfAttack_f(float* aoa) {
state.angle_of_attack_f = *aoa;
/* clear bits for all AOA representations and only set the new one */
@@ -1164,7 +1164,7 @@ static inline void stateSetAngleOfAttack_f(float* aoa) {
SetBit(state.wind_air_status, AOA_F);
}
-/// Set angle of attack (float).
+/// Set sideslip angle in radians (float).
static inline void stateSetSideslip_f(float* sideslip) {
state.sideslip_f = *sideslip;
/* clear bits for all sideslip representations and only set the new one */
diff --git a/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c b/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c
index 0f04e5b27a..42b1d788d2 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c
@@ -27,6 +27,11 @@
* and also sets battery level.
*/
+#ifdef ARDRONE2_DEBUG
+# include
+# include
+#endif
+
#include "ahrs_ardrone2.h"
#include "state.h"
#include "math/pprz_algebra_float.h"
@@ -80,15 +85,42 @@ void ahrs_align(void) {
}
+#ifdef ARDRONE2_DEBUG
+static void dump(const void *_b, size_t s) {
+ const unsigned char *b = _b;
+ size_t n;
+
+ for(n = 0; n < s; ++n) {
+ printf("%02x ", b[n]);
+ if (n%16 == 15)
+ printf("\n");
+ }
+ if (n%16 != 0)
+ printf("\n");
+}
+#endif
+
void ahrs_propagate(void) {
+ int l;
+
//Recieve the main packet
- at_com_recieve_navdata(buffer);
+ l = at_com_recieve_navdata(buffer);
navdata_t* main_packet = (navdata_t*) &buffer;
+#ifdef ARDRONE2_DEBUG
+ if (l < 0)
+ printf("errno = %d\n", errno);
+#endif
+
//When this isn't a valid packet return
- if(main_packet->header != NAVDATA_HEADER)
+ if(l < 0 || main_packet->header != NAVDATA_HEADER)
return;
+#ifdef ARDRONE2_DEBUG
+ printf("Read %d\n", l);
+ dump(buffer, l);
+#endif
+
//Set the state
ahrs_impl.state = main_packet->ardrone_state;
@@ -103,6 +135,9 @@ void ahrs_propagate(void) {
//Read the navdata until packet is fully readed
while(!full_read && navdata_option->size > 0) {
+#ifdef ARDRONE2_DEBUG
+ printf ("tag = %d\n", navdata_option->tag);
+#endif
//Check the tag for the right option
switch(navdata_option->tag) {
case 0: //NAVDATA_DEMO
@@ -137,6 +172,9 @@ void ahrs_propagate(void) {
break;
#ifdef USE_GPS_ARDRONE2
case 27: //NAVDATA_GPS
+# ifdef ARDRONE2_DEBUG
+ dump(navdata_option, navdata_option->size);
+# endif
navdata_gps = (navdata_gps_t*) navdata_option;
// Send the data to the gps parser
@@ -148,7 +186,9 @@ void ahrs_propagate(void) {
full_read = TRUE;
break;
default:
- //printf("NAVDATA UNKNOWN TAG: %d %d\n", navdata_option->tag, navdata_option->size);
+#ifdef ARDRONE2_DEBUG
+ printf("NAVDATA UNKNOWN TAG: %d %d\n", navdata_option->tag, navdata_option->size);
+#endif
break;
}
navdata_option = (navdata_option_t*) ((uint32_t)navdata_option + navdata_option->size);
diff --git a/sw/airborne/subsystems/ahrs/ahrs_extern_quat.c b/sw/airborne/subsystems/ahrs/ahrs_extern_quat.c
deleted file mode 100644
index b778af0233..0000000000
--- a/sw/airborne/subsystems/ahrs/ahrs_extern_quat.c
+++ /dev/null
@@ -1,85 +0,0 @@
-/*
- * Copyright (C) 2013 Michal Podhradsky
- * Utah State University, http://aggieair.usu.edu/
- *
- * 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 subsystems/ahrs/ahrs_extern_quat.c
- *
- * AHRS interface for multiple IMU/AHRS subsystems, such as GX3, UM6 etc.
- *
- * Propagates the estimated attitude and rates from IMU to body states. Quaternion
- * calculation is used.
- *
- * @author Michal Podhradsky
- */
-#include "ahrs_extern_quat.h"
-#include "mcu_periph/sys_time.h"
-#include "led.h"
-
-struct AhrsIntExternQuat ahrs_impl;
-
-void ahrs_init(void) {
- ahrs.status = AHRS_UNINIT;
-
- /* set ltp_to_imu so that body is zero */
- QUAT_COPY(ahrs_impl.ltp_to_imu_quat, imu.body_to_imu_quat);
- INT_RATES_ZERO(ahrs_impl.imu_rate);
-
- #ifdef IMU_MAG_OFFSET
- ahrs_impl.mag_offset = IMU_MAG_OFFSET;
- #else
- ahrs_impl.mag_offset = 0.;
- #endif
-
- //Needed to set orientations
- ahrs.status = AHRS_RUNNING;
-
- #ifdef AHRS_ALIGNER_LED
- LED_ON(AHRS_ALIGNER_LED);
- #endif
-}
-
-void ahrs_propagate(void) {
- /* Compute LTP to BODY quaternion */
- struct Int32Quat ltp_to_body_quat;
- INT32_QUAT_COMP_INV(ltp_to_body_quat, ahrs_impl.ltp_to_imu_quat, imu.body_to_imu_quat);
- stateSetNedToBodyQuat_i(<p_to_body_quat);
-
- // TODO: compensate for magnetic offset
-
- struct Int32Rates body_rate;
- ahrs_impl.imu_rate = imu.gyro;
- /* compute body rates */
- INT32_RMAT_TRANSP_RATEMULT(body_rate, imu.body_to_imu_rmat, ahrs_impl.imu_rate);
- /* Set state */
- stateSetBodyRates_i(&body_rate);
-}
-
-void ahrs_align(void) {
-}
-
-void ahrs_update_accel(void) {
-}
-
-void ahrs_update_mag(void) {
-}
-
-void ahrs_update_gps(void) {
-}
diff --git a/sw/airborne/subsystems/ahrs/ahrs_extern_quat.h b/sw/airborne/subsystems/ahrs/ahrs_extern_quat.h
deleted file mode 100644
index 74a415f387..0000000000
--- a/sw/airborne/subsystems/ahrs/ahrs_extern_quat.h
+++ /dev/null
@@ -1,48 +0,0 @@
-/*
- * Copyright (C) 2013 Michal Podhradsky
- * Utah State University, http://aggieair.usu.edu/
- *
- * 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 subsystems/ahrs/ahrs_extern_quat.h
- *
- * AHRS interface for multiple IMU/AHRS subsystems, such as GX3, UM6 etc.
- *
- * Propagates the estimated attitude and rates from IMU to body states. Quaternion
- * calculation is used.
- *
- * @author Michal Podhradsky
- */
-#ifndef AHRS_EXTERN_QUAT_H
-#define AHRS_EXTERN_QUAT_H
-
-#include "state.h"
-#include "subsystems/ahrs.h"
-#include "subsystems/imu.h"
-
-struct AhrsIntExternQuat {
- struct Int32Eulers ltp_to_imu_euler; ///< Rotation from LocalTangentPlane to IMU frame as Euler angles
- struct Int32Quat ltp_to_imu_quat; ///< Rotation from LocalTangentPlane to IMU frame as quaternions
- struct Int32Rates imu_rate; ///< Rotational velocity in IMU frame
- float mag_offset;
-};
-
-extern struct AhrsIntExternQuat ahrs_impl;
-
-#endif /* AHRS_EXTERN_QUAT_H */
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c
index c661f4c960..8965e1202c 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c
@@ -151,6 +151,8 @@ void ahrs_init(void) {
ahrs_impl.correct_gravity = FALSE;
#endif
+ VECT3_ASSIGN(ahrs_impl.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z);
+
#if DOWNLINK
register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_att);
#endif
@@ -287,9 +289,8 @@ void ahrs_update_mag(void) {
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_impl.ltp_to_imu_rmat, expected_ltp);
+ FLOAT_RMAT_VECT3_MUL(expected_imu, ahrs_impl.ltp_to_imu_rmat, ahrs_impl.mag_h);
struct FloatVect3 measured_imu;
MAGS_FLOAT_OF_BFP(measured_imu, imu.mag);
@@ -310,8 +311,6 @@ void ahrs_update_mag_full(void) {
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;
@@ -320,7 +319,7 @@ void ahrs_update_mag_2d(void) {
const struct FloatVect3 residual_ltp =
{ 0,
0,
- measured_ltp.x * expected_ltp.y - measured_ltp.y * expected_ltp.x };
+ measured_ltp.x * ahrs_impl.mag_h.y - measured_ltp.y * ahrs_impl.mag_h.x };
// printf("res : %f\n", residual_ltp.z);
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h
index a40d888f08..b14831fa3e 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h
@@ -44,6 +44,7 @@ struct AhrsFloatCmpl {
bool_t correct_gravity;
bool_t heading_aligned;
+ struct FloatVect3 mag_h;
/*
Holds float version of IMU alignement
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_ekf.c b/sw/airborne/subsystems/ahrs/ahrs_float_ekf.c
deleted file mode 100644
index 6ec8fb4eaf..0000000000
--- a/sw/airborne/subsystems/ahrs/ahrs_float_ekf.c
+++ /dev/null
@@ -1,164 +0,0 @@
-/*
- * Copyright (C) 2008-2009 Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- */
-
-#include "math/pprz_algebra_float.h"
-
-/* our estimated attitude */
-struct FloatQuat bafe_quat;
-/* our estimated gyro biases */
-struct FloatRates bafe_bias;
-/* we get unbiased body rates as byproduct */
-struct FloatRates bafe_rates;
-/* maintain a euler angles representation */
-struct FloatEulers bafe_eulers;
-/* maintains a rotation matrix representation */
-struct FloatEulers bafe_dcm;
-/* time derivative of our quaternion */
-struct FloatQuat bafe_qdot;
-
-#define BAFE_SSIZE 7
-/* covariance matrix and its time derivative */
-float bafe_P[BAFE_SSIZE][BAFE_SSIZE];
-float bafe_Pdot[BAFE_SSIZE][BAFE_SSIZE];
-
-/*
- * F represents the Jacobian of the derivative of the system with respect
- * its states. We do not allocate the bottom three rows since we know that
- * the derivatives of bias_dot are all zero.
- */
-float bafe_F[4][7];
-
-/*
- * Kalman filter variables.
- */
-float bafe_PHt[7];
-float bafe_K[7];
-float bafe_E;
-/*
- * H represents the Jacobian of the measurements of the attitude
- * with respect to the states of the filter. We do not allocate the bottom
- * three rows since we know that the attitude measurements have no
- * relationship to gyro bias.
- */
-float bafe_H[4];
-/* temporary variable used during state covariance update */
-float bafe_FP[4][7];
-
-/*
- * Q is our estimate noise variance. It is supposed to be an NxN
- * matrix, but with elements only on the diagonals. Additionally,
- * since the quaternion has no expected noise (we can't directly measure
- * it), those are zero. For the gyro, we expect around 5 deg/sec noise,
- * which is 0.08 rad/sec. The variance is then 0.08^2 ~= 0.0075.
- */
-/* I measured about 0.009 rad/s noise */
-#define bafe_Q_gyro 8e-03
-
-/*
- * R is our measurement noise estimate. Like Q, it is supposed to be
- * an NxN matrix with elements on the diagonals. However, since we can
- * not directly measure the gyro bias, we have no estimate for it.
- * We only have an expected noise in the pitch and roll accelerometers
- * and in the compass.
- */
-#define BAFE_R_PHI 1.3 * 1.3
-#define BAFE_R_THETA 1.3 * 1.3
-#define BAFE_R_PSI 2.5 * 2.5
-
-#ifndef BAFE_DT
-#define BAFE_DT (1./512.)
-#endif
-
-extern void ahrs_init(void);
-extern void ahrs_align(void);
-
-
-/*
- * Propagate our dynamic system
- *
- * quat_dot = Wxq(pqr) * quat
- * bias_dot = 0
- *
- * Wxq is the quaternion omega matrix:
- *
- * [ 0, -p, -q, -r ]
- * 1/2 * [ p, 0, r, -q ]
- * [ q, -r, 0, p ]
- * [ r, q, -p, 0 ]
- *
- *
- *
- *
- * [ 0 -p -q -r q1 q2 q3]
- * F = 1/2 * [ p 0 r -q -q0 q3 -q2]
- * [ q -r 0 p -q3 -q0 q1]
- * [ r q -p 0 q2 -q1 -q0]
- * [ 0 0 0 0 0 0 0]
- * [ 0 0 0 0 0 0 0]
- * [ 0 0 0 0 0 0 0]
- *
- */
-
-void ahrs_propagate(void) {
- /* compute unbiased rates */
- RATES_FLOAT_OF_BFP(bafe_rates, imu.gyro);
- RATES_SUB(bafe_rates, bafe_bias);
-
- /* compute F
- F is only needed later on to update the state covariance P.
- However, its [0:3][0:3] region is actually the Wxq(pqr) which is needed to
- compute the time derivative of the quaternion, so we compute F now
- */
-
- /* Fill in Wxq(pqr) into F */
- bafe_F[0][0] = bafe_F[1][1] = bafe_F[2][2] = bafe_F[3][3] = 0;
- bafe_F[1][0] = bafe_F[2][3] = bafe_rates.p * 0.5;
- bafe_F[2][0] = bafe_F[3][1] = bafe_rates.q * 0.5;
- bafe_F[3][0] = bafe_F[1][2] = bafe_rates.r * 0.5;
-
- bafe_F[0][1] = bafe_F[3][2] = -bafe_F[1][0];
- bafe_F[0][2] = bafe_F[1][3] = -bafe_F[2][0];
- bafe_F[0][3] = bafe_F[2][1] = -bafe_F[3][0];
- /* Fill in [4:6][0:3] region */
- bafe_F[0][4] = bafe_F[2][6] = bafe_quat.qx * 0.5;
- bafe_F[0][5] = bafe_F[3][4] = bafe_quat.qy * 0.5;
- bafe_F[0][6] = bafe_F[1][5] = bafe_quat.qz * 0.5;
-
- bafe_F[1][4] = bafe_F[2][5] = bafe_F[3][6] = bafe_quat.qi * -0.5;
- bafe_F[3][5] = -bafe_F[0][4];
- bafe_F[1][6] = -bafe_F[0][5];
- bafe_F[2][4] = -bafe_F[0][6];
- /* quat_dot = Wxq(pqr) * quat */
- bafe_qdot.qi= bafe_F[0][1]*bafe_quat.qx+bafe_F[0][2]*bafe_quat.qy+bafe_F[0][3] * bafe_quat.qz;
- bafe_qdot.qx= bafe_F[1][0]*bafe_quat.qi +bafe_F[1][2]*bafe_quat.qy+bafe_F[1][3] * bafe_quat.qz;
- bafe_qdot.qy= bafe_F[2][0]*bafe_quat.qi+bafe_F[2][1]*bafe_quat.qx +bafe_F[2][3] * bafe_quat.qz;
- bafe_qdot.qz= bafe_F[3][0]*bafe_quat.qi+bafe_F[3][1]*bafe_quat.qx+bafe_F[3][2]*bafe_quat.qy ;
- /* propagate quaternion */
- bafe_quat.qi += bafe_qdot.qi * BAFE_DT;
- bafe_quat.qx += bafe_qdot.qx * BAFE_DT;
- bafe_quat.qy += bafe_qdot.qy * BAFE_DT;
- bafe_quat.qz += bafe_qdot.qz * BAFE_DT;
-
-
-}
-
-
-extern void ahrs_update(void);
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c b/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c
deleted file mode 100644
index 92424b9990..0000000000
--- a/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c
+++ /dev/null
@@ -1,927 +0,0 @@
-/*
- * Copyright (C) 2009 Felix Ruess
- * 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.
- */
-
-/**
- * @file subsystems/ahrs/ahrs_float_lkf.c
- *
- * Linearized Kalman Filter for attitude estimation.
- *
- */
-
-#include "ahrs_float_lkf.h"
-
-#include "subsystems/imu.h"
-#include "subsystems/ahrs/ahrs_aligner.h"
-
-#include "generated/airframe.h"
-#include "math/pprz_algebra_float.h"
-
-#include
-
-
-#define BAFL_DEBUG
-
-static void ahrs_do_update_accel(void);
-static void ahrs_do_update_mag(void);
-
-
-/* our estimated attitude (ltp <-> imu) */
-struct FloatQuat bafl_quat;
-/* our estimated gyro biases */
-struct FloatRates bafl_bias;
-/* we get unbiased body rates as byproduct */
-struct FloatRates bafl_rates;
-/* maintain a euler angles representation */
-struct FloatEulers bafl_eulers;
-/* C n->b rotation matrix representation */
-struct FloatRMat bafl_dcm;
-
-/* estimated attitude errors from accel update */
-struct FloatQuat bafl_q_a_err;
-/* estimated attitude errors from mag update */
-struct FloatQuat bafl_q_m_err;
-/* our estimated gyro bias errors from accel update */
-struct FloatRates bafl_b_a_err;
-/* our estimated gyro bias errors from mag update */
-struct FloatRates bafl_b_m_err;
-/* temporary quaternion */
-struct FloatQuat bafl_qtemp;
-/* correction quaternion of strapdown computation */
-struct FloatQuat bafl_qr;
-/* norm of attitude quaternion */
-float bafl_qnorm;
-
-/* pseude measured angles for debug */
-float bafl_phi_accel;
-float bafl_theta_accel;
-
-#ifndef BAFL_SSIZE
-#define BAFL_SSIZE 6
-#endif
-/* error covariance matrix */
-float bafl_P[BAFL_SSIZE][BAFL_SSIZE];
-/* filter state */
-float bafl_X[BAFL_SSIZE];
-
-/*
- * F represents the Jacobian of the derivative of the system with respect
- * its states. We do not allocate the bottom three rows since we know that
- * the derivatives of bias_dot are all zero.
- */
-float bafl_F[3][3];
-/*
- * T represents the discrete state transition matrix
- * T = e^(F * dt)
- */
-float bafl_T[6][6];
-
-/*
- * Kalman filter variables.
- */
-float bafl_Pprio[BAFL_SSIZE][BAFL_SSIZE];
-/* temporary variable used during state covariance update */
-float bafl_tempP[BAFL_SSIZE][BAFL_SSIZE];
-float bafl_K[6][3];
-float bafl_tempK[6][3];
-float bafl_S[3][3];
-float bafl_tempS[3][6];
-float bafl_invS[3][3];
-struct FloatVect3 bafl_ya;
-struct FloatVect3 bafl_ym;
-
-/*
- * H represents the Jacobian of the measurements of the attitude
- * with respect to the states of the filter. We do not allocate the bottom
- * three rows since we know that the attitude measurements have no
- * relationship to gyro bias.
- * The last three columns always stay zero.
- */
-float bafl_H[3][3];
-
-/* low pass of accel measurements */
-struct FloatVect3 bafl_accel_measure;
-struct FloatVect3 bafl_accel_last;
-
-struct FloatVect3 bafl_mag;
-
-/* temporary variables for the strapdown computation */
-float bafl_qom[4][4];
-
-#define BAFL_g 9.81
-
-#define BAFL_hx 1.0
-#define BAFL_hy 0.0
-#define BAFL_hz 1.0
-struct FloatVect3 bafl_h;
-
-/*
- * Q is our estimate noise variance. It is supposed to be an NxN
- * matrix, but with elements only on the diagonals. Additionally,
- * since the quaternion has no expected noise (we can't directly measure
- * it), those are zero. For the gyro, we expect around 5 deg/sec noise,
- * which is 0.08 rad/sec. The variance is then 0.08^2 ~= 0.0075.
- */
-/* I measured about 0.009 rad/s noise */
-#define BAFL_Q_GYRO 1e-04
-#define BAFL_Q_ATT 0
-float bafl_Q_gyro;
-float bafl_Q_att;
-
-/*
- * R is our measurement noise estimate. Like Q, it is supposed to be
- * an NxN matrix with elements on the diagonals. However, since we can
- * not directly measure the gyro bias, we have no estimate for it.
- * We only have an expected noise in the pitch and roll accelerometers
- * and in the compass.
- */
-#define BAFL_SIGMA_ACCEL 1000.0
-#define BAFL_SIGMA_MAG 20.
-float bafl_sigma_accel;
-float bafl_sigma_mag;
-float bafl_R_accel;
-float bafl_R_mag;
-
-#ifndef BAFL_DT
-#define BAFL_DT (1./512.)
-#endif
-
-
-#define FLOAT_MAT33_INVERT(_mo, _mi) { \
- float _det = 0.; \
- _det = _mi[0][0] * (_mi[2][2] * _mi[1][1] - _mi[2][1] * _mi[1][2]) \
- - _mi[1][0] * (_mi[2][2] * _mi[0][1] - _mi[2][1] * _mi[0][2]) \
- + _mi[2][0] * (_mi[1][2] * _mi[0][1] - _mi[1][1] * _mi[0][2]); \
- if (_det != 0.) { /* ? test for zero? */ \
- _mo[0][0] = (_mi[1][1] * _mi[2][2] - _mi[1][2] * _mi[2][1]) / _det; \
- _mo[0][1] = (_mi[0][2] * _mi[2][1] - _mi[0][1] * _mi[2][2]) / _det; \
- _mo[0][2] = (_mi[0][1] * _mi[1][2] - _mi[0][2] * _mi[1][1]) / _det; \
- \
- _mo[1][0] = (_mi[1][2] * _mi[2][0] - _mi[1][0] * _mi[2][2]) / _det; \
- _mo[1][1] = (_mi[0][0] * _mi[2][2] - _mi[0][2] * _mi[2][0]) / _det; \
- _mo[1][2] = (_mi[0][2] * _mi[1][0] - _mi[0][0] * _mi[1][2]) / _det; \
- \
- _mo[2][0] = (_mi[1][0] * _mi[2][1] - _mi[1][1] * _mi[2][0]) / _det; \
- _mo[2][1] = (_mi[0][1] * _mi[2][0] - _mi[0][0] * _mi[2][1]) / _det; \
- _mo[2][2] = (_mi[0][0] * _mi[1][1] - _mi[0][1] * _mi[1][0]) / _det; \
- } else { \
- printf("ERROR! Not invertible!\n"); \
- for (int _i=0; _i<3; _i++) { \
- for (int _j=0; _j<3; _j++) { \
- _mo[_i][_j] = 0.0; \
- } \
- } \
- } \
- }
-
-#define AHRS_TO_BFP() { \
- /* IMU rate */ \
- RATES_BFP_OF_REAL(ahrs.imu_rate, bafl_rates); \
- /* LTP to IMU eulers */ \
- EULERS_BFP_OF_REAL(ahrs.ltp_to_imu_euler, bafl_eulers); \
- /* LTP to IMU quaternion */ \
- QUAT_BFP_OF_REAL(ahrs.ltp_to_imu_quat, bafl_quat); \
- /* LTP to IMU rotation matrix */ \
- RMAT_BFP_OF_REAL(ahrs.ltp_to_imu_rmat, bafl_dcm); \
- }
-
-#define AHRS_LTP_TO_BODY() { \
- /* 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); \
- }
-
-#if DOWNLINK
-#include "subsystems/datalink/telemetry.h"
-
-static void send_lkf(void) {
- DOWNLINK_SEND_AHRS_LKF(&bafl_eulers.phi,
- DefaultChannel, DefaultDevice,
- &bafl_eulers.theta,
- &bafl_eulers.psi,
- &bafl_quat.qi,
- &bafl_quat.qx,
- &bafl_quat.qy,
- &bafl_quat.qz,
- &bafl_rates.p,
- &bafl_rates.q,
- &bafl_rates.r,
- &bafl_accel_measure.x,
- &bafl_accel_measure.y,
- &bafl_accel_measure.z,
- &bafl_mag.x,
- &bafl_mag.y,
- &bafl_mag.z);
-}
-
-static void send_lkf_debug(void) {
- DOWNLINK_SEND_AHRS_LKF_DEBUG(DefaultChannel, DefaultDevice,
- &bafl_X[0],
- &bafl_X[1],
- &bafl_X[2],
- &bafl_bias.p,
- &bafl_bias.q,
- &bafl_bias.r,
- &bafl_qnorm,
- &bafl_phi_accel,
- &bafl_theta_accel,
- &bafl_P[0][0],
- &bafl_P[1][1],
- &bafl_P[2][2],
- &bafl_P[3][3],
- &bafl_P[4][4],
- &bafl_P[5][5]);
-}
-
-static void send_lkf_acc(void) {
- DOWNLINK_SEND_AHRS_LKF_ACC_DBG(DefaultChannel, DefaultDevice,
- &bafl_q_a_err.qi,
- &bafl_q_a_err.qx,
- &bafl_q_a_err.qy,
- &bafl_q_a_err.qz,
- &bafl_b_a_err.p,
- &bafl_b_a_err.q,
- &bafl_b_a_err.r);
-}
-
-static void send_lkf_mag(void) {
- DOWNLINK_SEND_AHRS_LKF_MAG_DBG(DefaultChannel, DefaultDevice,
- &bafl_q_m_err.qi,
- &bafl_q_m_err.qx,
- &bafl_q_m_err.qy,
- &bafl_q_m_err.qz,
- &bafl_b_m_err.p,
- &bafl_b_m_err.q,
- &bafl_b_m_err.r);
-}
-#endif
-
-void ahrs_init(void) {
- int i, j;
-
- for (i = 0; i < BAFL_SSIZE; i++) {
- for (j = 0; j < BAFL_SSIZE; j++) {
- bafl_T[i][j] = 0.;
- bafl_P[i][j] = 0.;
- }
- /* Insert the diagonal elements of the T-Matrix. These will never change. */
- bafl_T[i][i] = 1.0;
- /* initial covariance values */
- if (i<3) {
- bafl_P[i][i] = 1.0;
- } else {
- bafl_P[i][i] = 0.1;
- }
- }
-
- FLOAT_QUAT_ZERO(bafl_quat);
-
- ahrs.status = AHRS_UNINIT;
- INT_EULERS_ZERO(ahrs.ltp_to_body_euler);
- INT_EULERS_ZERO(ahrs.ltp_to_imu_euler);
- INT32_QUAT_ZERO(ahrs.ltp_to_body_quat);
- INT32_QUAT_ZERO(ahrs.ltp_to_imu_quat);
- INT_RATES_ZERO(ahrs.body_rate);
- INT_RATES_ZERO(ahrs.imu_rate);
-
- ahrs_float_lkf_SetRaccel(BAFL_SIGMA_ACCEL);
- ahrs_float_lkf_SetRmag(BAFL_SIGMA_MAG);
-
- bafl_Q_att = BAFL_Q_ATT;
- bafl_Q_gyro = BAFL_Q_GYRO;
-
- FLOAT_VECT3_ASSIGN(bafl_h, BAFL_hx,BAFL_hy, BAFL_hz);
-
-#if DOWNLINK
- register_periodic_telemetry(DefaultPeriodic, "AHRS_LKF", send_lkf);
- register_periodic_telemetry(DefaultPeriodic, "AHRS_LKF_DEBUG", send_lkf_debug);
- register_periodic_telemetry(DefaultPeriodic, "AHRS_LKF_ACC_DBG", send_lkf_acc);
- register_periodic_telemetry(DefaultPeriodic, "AHRS_LKF_MAG_DBG", send_lkf_mag);
-#endif
-}
-
-void ahrs_align(void) {
- RATES_FLOAT_OF_BFP(bafl_bias, ahrs_aligner.lp_gyro);
- ACCELS_FLOAT_OF_BFP(bafl_accel_measure, ahrs_aligner.lp_accel);
- ahrs.status = AHRS_RUNNING;
-}
-
-static inline void ahrs_lowpass_accel(void) {
- // get latest measurement
- ACCELS_FLOAT_OF_BFP(bafl_accel_last, imu.accel);
- // low pass it
- VECT3_ADD(bafl_accel_measure, bafl_accel_last);
- VECT3_SDIV(bafl_accel_measure, bafl_accel_measure, 2);
-
-#ifdef BAFL_DEBUG
- bafl_phi_accel = atan2f( -bafl_accel_measure.y, -bafl_accel_measure.z);
- bafl_theta_accel = atan2f(bafl_accel_measure.x, sqrtf(bafl_accel_measure.y*bafl_accel_measure.y + bafl_accel_measure.z*bafl_accel_measure.z));
-#endif
-}
-
-/*
- * Propagate our dynamic system in time
- *
- * Run the strapdown computation and the predict step of the filter
- *
- *
- * quat_dot = Wxq(pqr) * quat
- * bias_dot = 0
- *
- * Wxq is the quaternion omega matrix:
- *
- * [ 0, -p, -q, -r ]
- * 1/2 * [ p, 0, r, -q ]
- * [ q, -r, 0, p ]
- * [ r, q, -p, 0 ]
- *
- */
-void ahrs_propagate(void) {
- int i, j, k;
-
- ahrs_lowpass_accel();
-
- /* compute unbiased rates */
- RATES_FLOAT_OF_BFP(bafl_rates, imu.gyro);
- RATES_SUB(bafl_rates, bafl_bias);
-
-
- /* run strapdown computation
- *
- */
-
- /* multiplicative quaternion update */
- /* compute correction quaternion */
- QUAT_ASSIGN(bafl_qr, 1., bafl_rates.p * BAFL_DT / 2, bafl_rates.q * BAFL_DT / 2, bafl_rates.r * BAFL_DT / 2);
- /* normalize it. maybe not necessary?? */
- float q_sq;
- q_sq = (bafl_qr.qx * bafl_qr.qx +bafl_qr.qy * bafl_qr.qy + bafl_qr.qz * bafl_qr.qz) / 4;
- if (q_sq > 1) { /* this should actually never happen */
- FLOAT_QUAT_SMUL(bafl_qr, bafl_qr, 1 / sqrtf(1 + q_sq));
- } else {
- bafl_qr.qi = sqrtf(1 - q_sq);
- }
- /* propagate correction quaternion */
- FLOAT_QUAT_COMP(bafl_qtemp, bafl_quat, bafl_qr);
- FLOAT_QUAT_COPY(bafl_quat, bafl_qtemp);
-
- bafl_qnorm = FLOAT_QUAT_NORM(bafl_quat);
- //TODO check if broot force normalization is good, use lagrange normalization?
- FLOAT_QUAT_NORMALIZE(bafl_quat);
-
-
- /*
- * compute all representations
- */
- /* maintain rotation matrix representation */
- FLOAT_RMAT_OF_QUAT(bafl_dcm, bafl_quat);
- /* maintain euler representation */
- FLOAT_EULERS_OF_RMAT(bafl_eulers, bafl_dcm);
- AHRS_TO_BFP();
- AHRS_LTP_TO_BODY();
-
-
- /* error KF-Filter
- * propagate only the error covariance matrix since error is corrected after each measurement
- *
- * F = [ 0 0 0 ]
- * [ 0 0 0 -Cbn ]
- * [ 0 0 0 ]
- * [ 0 0 0 0 0 0 ]
- * [ 0 0 0 0 0 0 ]
- * [ 0 0 0 0 0 0 ]
- *
- * T = e^(dt * F)
- *
- * T = [ 1 0 0 ]
- * [ 0 1 0 -Cbn*dt ]
- * [ 0 0 1 ]
- * [ 0 0 0 1 0 0 ]
- * [ 0 0 0 0 1 0 ]
- * [ 0 0 0 0 0 1 ]
- *
- * P_prio = T * P * T_T + Q
- *
- */
-
- /*
- * compute state transition matrix T
- *
- * diagonal elements of T are always one
- */
- for (i = 0; i < 3; i++) {
- for (j = 0; j < 3; j++) {
- bafl_T[i][j + 3] = -RMAT_ELMT(bafl_dcm, j, i); /* inverted bafl_dcm */
- }
- }
-
-
- /*
- * estimate the a priori error covariance matrix P_k = T * P_k-1 * T_T + Q
- */
- /* Temporary multiplication temp(6x6) = T(6x6) * P(6x6) */
- for (i = 0; i < BAFL_SSIZE; i++) {
- for (j = 0; j < BAFL_SSIZE; j++) {
- bafl_tempP[i][j] = 0.;
- for (k = 0; k < BAFL_SSIZE; k++) {
- bafl_tempP[i][j] += bafl_T[i][k] * bafl_P[k][j];
- }
- }
- }
-
- /* P_k(6x6) = temp(6x6) * T_T(6x6) + Q */
- for (i = 0; i < BAFL_SSIZE; i++) {
- for (j = 0; j < BAFL_SSIZE; j++) {
- bafl_P[i][j] = 0.;
- for (k = 0; k < BAFL_SSIZE; k++) {
- bafl_P[i][j] += bafl_tempP[i][k] * bafl_T[j][k]; //T[j][k] = T_T[k][j]
- }
- }
- if (i<3) {
- bafl_P[i][i] += bafl_Q_att;
- } else {
- bafl_P[i][i] += bafl_Q_gyro;
- }
- }
-
-#ifdef LKF_PRINT_P
- printf("Pp =");
- for (i = 0; i < 6; i++) {
- printf("[");
- for (j = 0; j < 6; j++) {
- printf("%f\t", bafl_P[i][j]);
- }
- printf("]\n ");
- }
- printf("\n");
-#endif
-}
-
-void ahrs_update_accel(void) {
- RunOnceEvery(50, ahrs_do_update_accel());
-}
-
-static void ahrs_do_update_accel(void) {
- int i, j, k;
-
-#ifdef BAFL_DEBUG2
- printf("Accel update.\n");
-#endif
-
- /* P_prio = P */
- for (i = 0; i < BAFL_SSIZE; i++) {
- for (j = 0; j < BAFL_SSIZE; j++) {
- bafl_Pprio[i][j] = bafl_P[i][j];
- }
- }
-
- /*
- * set up measurement matrix
- *
- * g = 9.81
- * gx = [ 0 -g 0
- * g 0 0
- * 0 0 0 ]
- * H = [ 0 0 0 ]
- * [ -Cnb*gx 0 0 0 ]
- * [ 0 0 0 ]
- *
- * */
- bafl_H[0][0] = -RMAT_ELMT(bafl_dcm, 0, 1) * BAFL_g;
- bafl_H[0][1] = RMAT_ELMT(bafl_dcm, 0, 0) * BAFL_g;
- bafl_H[1][0] = -RMAT_ELMT(bafl_dcm, 1, 1) * BAFL_g;
- bafl_H[1][1] = RMAT_ELMT(bafl_dcm, 1, 0) * BAFL_g;
- bafl_H[2][0] = -RMAT_ELMT(bafl_dcm, 2, 1) * BAFL_g;
- bafl_H[2][1] = RMAT_ELMT(bafl_dcm, 2, 0) * BAFL_g;
- /* rest is zero
- bafl_H[0][2] = 0.;
- bafl_H[1][2] = 0.;
- bafl_H[2][2] = 0.;*/
-
- /**********************************************
- * compute Kalman gain K
- *
- * K = P_prio * H_T * inv(S)
- * S = H * P_prio * HT + R
- *
- * K = P_prio * HT * inv(H * P_prio * HT + R)
- *
- **********************************************/
-
- /* covariance residual S = H * P_prio * HT + R */
-
- /* temp_S(3x6) = H(3x6) * P_prio(6x6)
- * last 4 columns of H are zero
- */
- for (i = 0; i < 3; i++) {
- for (j = 0; j < BAFL_SSIZE; j++) {
- bafl_tempS[i][j] = bafl_H[i][0] * bafl_Pprio[0][j];
- bafl_tempS[i][j] += bafl_H[i][1] * bafl_Pprio[1][j];
- }
- }
-
- /* S(3x3) = temp_S(3x6) * HT(6x3) + R(3x3)
- *
- * bottom 4 rows of HT are zero
- */
- for (i = 0; i < 3; i++) {
- for (j = 0; j < 3; j++) {
- bafl_S[i][j] = bafl_tempS[i][0] * bafl_H[j][0]; /* H[j][0] = HT[0][j] */
- bafl_S[i][j] += bafl_tempS[i][1] * bafl_H[j][1]; /* H[j][0] = HT[0][j] */
- }
- bafl_S[i][i] += bafl_R_accel;
- }
-
- /* invert S
- */
- FLOAT_MAT33_INVERT(bafl_invS, bafl_S);
-
-
- /* temp_K(6x3) = P_prio(6x6) * HT(6x3)
- *
- * bottom 4 rows of HT are zero
- */
- for (i = 0; i < BAFL_SSIZE; i++) {
- for (j = 0; j < 3; j++) {
- /* not computing zero elements */
- bafl_tempK[i][j] = bafl_Pprio[i][0] * bafl_H[j][0]; /* H[j][0] = HT[0][j] */
- bafl_tempK[i][j] += bafl_Pprio[i][1] * bafl_H[j][1]; /* H[j][1] = HT[1][j] */
- }
- }
-
- /* K(6x3) = temp_K(6x3) * invS(3x3)
- */
- for (i = 0; i < BAFL_SSIZE; i++) {
- for (j = 0; j < 3; j++) {
- bafl_K[i][j] = bafl_tempK[i][0] * bafl_invS[0][j];
- bafl_K[i][j] += bafl_tempK[i][1] * bafl_invS[1][j];
- bafl_K[i][j] += bafl_tempK[i][2] * bafl_invS[2][j];
- }
- }
-
- /**********************************************
- * Update filter state.
- *
- * The a priori filter state is zero since the errors are corrected after each update.
- *
- * X = X_prio + K (y - H * X_prio)
- * X = K * y
- **********************************************/
-
- /*printf("R = ");
- for (i = 0; i < 3; i++) {
- printf("[");
- for (j = 0; j < 3; j++) {
- printf("%f\t", RMAT_ELMT(bafl_dcm, i, j));
- }
- printf("]\n ");
- }
- printf("\n");*/
-
- /* innovation
- * y = Cnb * -[0; 0; g] - accel
- */
- bafl_ya.x = -RMAT_ELMT(bafl_dcm, 0, 2) * BAFL_g - bafl_accel_measure.x;
- bafl_ya.y = -RMAT_ELMT(bafl_dcm, 1, 2) * BAFL_g - bafl_accel_measure.y;
- bafl_ya.z = -RMAT_ELMT(bafl_dcm, 2, 2) * BAFL_g - bafl_accel_measure.z;
-
- /* X(6) = K(6x3) * y(3)
- */
- for (i = 0; i < BAFL_SSIZE; i++) {
- bafl_X[i] = bafl_K[i][0] * bafl_ya.x;
- bafl_X[i] += bafl_K[i][1] * bafl_ya.y;
- bafl_X[i] += bafl_K[i][2] * bafl_ya.z;
- }
-
- /**********************************************
- * Update the filter covariance.
- *
- * P = P_prio - K * H * P_prio
- * P = ( I - K * H ) * P_prio
- *
- **********************************************/
-
- /* temp(6x6) = I(6x6) - K(6x3)*H(3x6)
- *
- * last 4 columns of H are zero
- */
- for (i = 0; i < BAFL_SSIZE; i++) {
- for (j = 0; j < BAFL_SSIZE; j++) {
- if (i == j) {
- bafl_tempP[i][j] = 1.;
- } else {
- bafl_tempP[i][j] = 0.;
- }
- if (j < 2) { /* omit the parts where H is zero */
- for (k = 0; k < 3; k++) {
- bafl_tempP[i][j] -= bafl_K[i][k] * bafl_H[k][j];
- }
- }
- }
- }
-
- /* P(6x6) = temp(6x6) * P_prio(6x6)
- */
- for (i = 0; i < BAFL_SSIZE; i++) {
- for (j = 0; j < BAFL_SSIZE; j++) {
- bafl_P[i][j] = bafl_tempP[i][0] * bafl_Pprio[0][j];
- for (k = 1; k < BAFL_SSIZE; k++) {
- bafl_P[i][j] += bafl_tempP[i][k] * bafl_Pprio[k][j];
- }
- }
- }
-
-#ifdef LKF_PRINT_P
- printf("Pua=");
- for (i = 0; i < 6; i++) {
- printf("[");
- for (j = 0; j < 6; j++) {
- printf("%f\t", bafl_P[i][j]);
- }
- printf("]\n ");
- }
- printf("\n");
-#endif
-
- /**********************************************
- * Correct errors.
- *
- *
- **********************************************/
-
- /* Error quaternion.
- */
- QUAT_ASSIGN(bafl_q_a_err, 1.0, bafl_X[0]/2, bafl_X[1]/2, bafl_X[2]/2);
- FLOAT_QUAT_INVERT(bafl_q_a_err, bafl_q_a_err);
-
- /* normalize
- * Is this needed? Or is the approximation good enough?*/
- float q_sq;
- q_sq = bafl_q_a_err.qx * bafl_q_a_err.qx + bafl_q_a_err.qy * bafl_q_a_err.qy + bafl_q_a_err.qz * bafl_q_a_err.qz;
- if (q_sq > 1) { /* this should actually never happen */
- FLOAT_QUAT_SMUL(bafl_q_a_err, bafl_q_a_err, 1 / sqrtf(1 + q_sq));
- printf("Accel error quaternion q_sq > 1!!\n");
- } else {
- bafl_q_a_err.qi = sqrtf(1 - q_sq);
- }
-
- /* correct attitude
- */
- FLOAT_QUAT_COMP(bafl_qtemp, bafl_q_a_err, bafl_quat);
- FLOAT_QUAT_COPY(bafl_quat, bafl_qtemp);
-
-
- /* correct gyro bias
- */
- RATES_ASSIGN(bafl_b_a_err, bafl_X[3], bafl_X[4], bafl_X[5]);
- RATES_SUB(bafl_bias, bafl_b_a_err);
-
- /*
- * compute all representations
- */
- /* maintain rotation matrix representation */
- FLOAT_RMAT_OF_QUAT(bafl_dcm, bafl_quat);
- /* maintain euler representation */
- FLOAT_EULERS_OF_RMAT(bafl_eulers, bafl_dcm);
- AHRS_TO_BFP();
- AHRS_LTP_TO_BODY();
-}
-
-
-void ahrs_update_mag(void) {
- RunOnceEvery(10, ahrs_do_update_mag());
-}
-
-static void ahrs_do_update_mag(void) {
- int i, j, k;
-
-#ifdef BAFL_DEBUG2
- printf("Mag update.\n");
-#endif
-
- MAGS_FLOAT_OF_BFP(bafl_mag, imu.mag);
-
- /* P_prio = P */
- for (i = 0; i < BAFL_SSIZE; i++) {
- for (j = 0; j < BAFL_SSIZE; j++) {
- bafl_Pprio[i][j] = bafl_P[i][j];
- }
- }
-
- /*
- * set up measurement matrix
- *
- * h = [236.; -2.; 396.];
- * hx = [ h(2); -h(1); 0]; //only psi (phi and theta = 0)
- * Hm = Cnb * hx;
- * H = [ 0 0 0 0 0
- * 0 0 Cnb*hx 0 0 0
- * 0 0 0 0 0 ];
- * */
- /*bafl_H[0][2] = RMAT_ELMT(bafl_dcm, 0, 0) * bafl_h.y - RMAT_ELMT(bafl_dcm, 0, 1) * bafl_h.x;
- bafl_H[1][2] = RMAT_ELMT(bafl_dcm, 1, 0) * bafl_h.y - RMAT_ELMT(bafl_dcm, 1, 1) * bafl_h.x;
- bafl_H[2][2] = RMAT_ELMT(bafl_dcm, 2, 0) * bafl_h.y - RMAT_ELMT(bafl_dcm, 2, 1) * bafl_h.x;*/
- bafl_H[0][2] = -RMAT_ELMT(bafl_dcm, 0, 1);
- bafl_H[1][2] = -RMAT_ELMT(bafl_dcm, 1, 1);
- bafl_H[2][2] = -RMAT_ELMT(bafl_dcm, 2, 1);
- //rest is zero
-
- /**********************************************
- * compute Kalman gain K
- *
- * K = P_prio * H_T * inv(S)
- * S = H * P_prio * HT + R
- *
- * K = P_prio * HT * inv(H * P_prio * HT + R)
- *
- **********************************************/
-
- /* covariance residual S = H * P_prio * HT + R */
-
- /* temp_S(3x6) = H(3x6) * P_prio(6x6)
- *
- * only third column of H is non-zero
- */
- for (i = 0; i < 3; i++) {
- for (j = 0; j < BAFL_SSIZE; j++) {
- bafl_tempS[i][j] = bafl_H[i][2] * bafl_Pprio[2][j];
- }
- }
-
- /* S(3x3) = temp_S(3x6) * HT(6x3) + R(3x3)
- *
- * only third row of HT is non-zero
- */
- for (i = 0; i < 3; i++) {
- for (j = 0; j < 3; j++) {
- bafl_S[i][j] = bafl_tempS[i][2] * bafl_H[j][2]; /* H[j][2] = HT[2][j] */
- }
- bafl_S[i][i] += bafl_R_mag;
- }
-
- /* invert S
- */
- FLOAT_MAT33_INVERT(bafl_invS, bafl_S);
-
- /* temp_K(6x3) = P_prio(6x6) * HT(6x3)
- *
- * only third row of HT is non-zero
- */
- for (i = 0; i < BAFL_SSIZE; i++) {
- for (j = 0; j < 3; j++) {
- /* not computing zero elements */
- bafl_tempK[i][j] = bafl_Pprio[i][2] * bafl_H[j][2]; /* H[j][2] = HT[2][j] */
- }
- }
-
- /* K(6x3) = temp_K(6x3) * invS(3x3)
- */
- for (i = 0; i < BAFL_SSIZE; i++) {
- for (j = 0; j < 3; j++) {
- bafl_K[i][j] = bafl_tempK[i][0] * bafl_invS[0][j];
- bafl_K[i][j] += bafl_tempK[i][1] * bafl_invS[1][j];
- bafl_K[i][j] += bafl_tempK[i][2] * bafl_invS[2][j];
- }
- }
-
- /**********************************************
- * Update filter state.
- *
- * The a priori filter state is zero since the errors are corrected after each update.
- *
- * X = X_prio + K (y - H * X_prio)
- * X = K * y
- **********************************************/
-
- /* innovation
- * y = Cnb * [hx; hy; hz] - mag
- */
- FLOAT_RMAT_VECT3_MUL(bafl_ym, bafl_dcm, bafl_h); //can be optimized
- FLOAT_VECT3_SUB(bafl_ym, bafl_mag);
-
- /* X(6) = K(6x3) * y(3)
- */
- for (i = 0; i < BAFL_SSIZE; i++) {
- bafl_X[i] = bafl_K[i][0] * bafl_ym.x;
- bafl_X[i] += bafl_K[i][1] * bafl_ym.y;
- bafl_X[i] += bafl_K[i][2] * bafl_ym.z;
- }
-
- /**********************************************
- * Update the filter covariance.
- *
- * P = P_prio - K * H * P_prio
- * P = ( I - K * H ) * P_prio
- *
- **********************************************/
-
- /* temp(6x6) = I(6x6) - K(6x3)*H(3x6)
- *
- * last 3 columns of H are zero
- */
- for (i = 0; i < 6; i++) {
- for (j = 0; j < 6; j++) {
- if (i == j) {
- bafl_tempP[i][j] = 1.;
- } else {
- bafl_tempP[i][j] = 0.;
- }
- if (j == 2) { /* omit the parts where H is zero */
- for (k = 0; k < 3; k++) {
- bafl_tempP[i][j] -= bafl_K[i][k] * bafl_H[k][j];
- }
- }
- }
- }
-
- /* P(6x6) = temp(6x6) * P_prio(6x6)
- */
- for (i = 0; i < BAFL_SSIZE; i++) {
- for (j = 0; j < BAFL_SSIZE; j++) {
- bafl_P[i][j] = bafl_tempP[i][0] * bafl_Pprio[0][j];
- for (k = 1; k < BAFL_SSIZE; k++) {
- bafl_P[i][j] += bafl_tempP[i][k] * bafl_Pprio[k][j];
- }
- }
- }
-
-#ifdef LKF_PRINT_P
- printf("Pum=");
- for (i = 0; i < 6; i++) {
- printf("[");
- for (j = 0; j < 6; j++) {
- printf("%f\t", bafl_P[i][j]);
- }
- printf("]\n ");
- }
- printf("\n");
-#endif
-
- /**********************************************
- * Correct errors.
- *
- *
- **********************************************/
-
- /* Error quaternion.
- */
- QUAT_ASSIGN(bafl_q_m_err, 1.0, bafl_X[0]/2, bafl_X[1]/2, bafl_X[2]/2);
- FLOAT_QUAT_INVERT(bafl_q_m_err, bafl_q_m_err);
- /* normalize */
- float q_sq;
- q_sq = bafl_q_m_err.qx * bafl_q_m_err.qx + bafl_q_m_err.qy * bafl_q_m_err.qy + bafl_q_m_err.qz * bafl_q_m_err.qz;
- if (q_sq > 1) { /* this should actually never happen */
- FLOAT_QUAT_SMUL(bafl_q_m_err, bafl_q_m_err, 1 / sqrtf(1 + q_sq));
- printf("mag error quaternion q_sq > 1!!\n");
- } else {
- bafl_q_m_err.qi = sqrtf(1 - q_sq);
- }
-
- /* correct attitude
- */
- FLOAT_QUAT_COMP(bafl_qtemp, bafl_q_m_err, bafl_quat);
- FLOAT_QUAT_COPY(bafl_quat, bafl_qtemp);
-
- /* correct gyro bias
- */
- RATES_ASSIGN(bafl_b_m_err, bafl_X[3], bafl_X[4], bafl_X[5]);
- RATES_SUB(bafl_bias, bafl_b_m_err);
-
- /*
- * compute all representations
- */
- /* maintain rotation matrix representation */
- FLOAT_RMAT_OF_QUAT(bafl_dcm, bafl_quat);
- /* maintain euler representation */
- FLOAT_EULERS_OF_RMAT(bafl_eulers, bafl_dcm);
- AHRS_TO_BFP();
- AHRS_LTP_TO_BODY();
-}
-
-void ahrs_update(void) {
- ahrs_update_accel();
- ahrs_update_mag();
-}
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_lkf.h b/sw/airborne/subsystems/ahrs/ahrs_float_lkf.h
deleted file mode 100644
index 71afc6d3cf..0000000000
--- a/sw/airborne/subsystems/ahrs/ahrs_float_lkf.h
+++ /dev/null
@@ -1,76 +0,0 @@
-/*
- * Copyright (C) 2009 Felix Ruess
- * 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.
- */
-
-/**
- * @file subsystems/ahrs/ahrs_float_lkf.h
- *
- * Linearized Kalman Filter for attitude estimation.
- *
- */
-
-#ifndef AHRS_FLOAT_LKF_H
-#define AHRS_FLOAT_LKF_H
-
-#include "subsystems/ahrs.h"
-#include "std.h"
-#include "math/pprz_algebra_int.h"
-
-extern struct FloatQuat bafl_quat;
-extern struct FloatRates bafl_bias;
-extern struct FloatRates bafl_rates;
-extern struct FloatEulers bafl_eulers;
-extern struct FloatRMat bafl_dcm;
-
-extern struct FloatQuat bafl_q_a_err;
-extern struct FloatQuat bafl_q_m_err;
-extern struct FloatRates bafl_b_a_err;
-extern struct FloatRates bafl_b_m_err;
-extern float bafl_qnorm;
-extern float bafl_phi_accel;
-extern float bafl_theta_accel;
-extern struct FloatVect3 bafl_accel_measure;
-extern struct FloatVect3 bafl_mag;
-
-#define BAFL_SSIZE 6
-extern float bafl_P[BAFL_SSIZE][BAFL_SSIZE];
-extern float bafl_X[BAFL_SSIZE];
-
-extern float bafl_sigma_accel;
-extern float bafl_sigma_mag;
-extern float bafl_R_accel;
-extern float bafl_R_mag;
-
-extern float bafl_Q_att;
-extern float bafl_Q_gyro;
-
-
-#define ahrs_float_lkf_SetRaccel(_v) { \
- bafl_sigma_accel = _v; \
- bafl_R_accel = _v * _v; \
-}
-#define ahrs_float_lkf_SetRmag(_v) { \
- bafl_sigma_mag = _v; \
- bafl_R_mag = _v * _v; \
-}
-
-#endif /* AHRS_FLOAT_LKF_H */
-
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c
index 2ad5bc5d43..8cc175a819 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.c
@@ -74,10 +74,11 @@ void ahrs_init(void) {
/* Set ltp_to_imu so that body is zero */
QUAT_COPY(ahrs_impl.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
- RMAT_COPY(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
FLOAT_RATES_ZERO(ahrs_impl.imu_rate);
+ VECT3_ASSIGN(ahrs_impl.mag_h, AHRS_H_X, AHRS_H_Y, AHRS_H_Z);
+
/*
* Initialises our state
*/
@@ -99,9 +100,6 @@ void ahrs_align(void) {
/* Compute an initial orientation from accel and mag directly as quaternion */
ahrs_float_get_quat_from_accel_mag(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
- /* Convert initial orientation from quat to rotation matrix representations. */
- FLOAT_RMAT_OF_QUAT(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat);
-
/* set initial body orientation */
set_body_state_from_quat();
@@ -119,6 +117,9 @@ void ahrs_propagate(void) {
set_body_state_from_quat();
}
+void ahrs_update_gps(void) {
+}
+
void ahrs_update_accel(void) {
struct FloatVect3 imu_g;
ACCELS_FLOAT_OF_BFP(imu_g, imu.accel);
@@ -137,9 +138,8 @@ void ahrs_update_accel(void) {
void ahrs_update_mag(void) {
struct FloatVect3 imu_h;
MAGS_FLOAT_OF_BFP(imu_h, imu.mag);
- const struct FloatVect3 earth_h = { AHRS_H_X , AHRS_H_Y, AHRS_H_Z };
const float h_noise[] = { 0.1610, 0.1771, 0.2659};
- update_state(&earth_h, &imu_h, h_noise);
+ update_state(&ahrs_impl.mag_h, &imu_h, h_noise);
reset_state();
}
@@ -161,30 +161,9 @@ static inline void propagate_ref(void) {
RATES_COPY(ahrs_impl.imu_rate, gyro_float);
#endif
-
- /* propagate reference quaternion only if rate is non null */
- const float no = FLOAT_RATES_NORM(ahrs_impl.imu_rate);
- if (no > FLT_MIN) {
- const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
- 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*ahrs_impl.imu_rate.p;
- const float dq = sa_ov_no*ahrs_impl.imu_rate.q;
- const float dr = sa_ov_no*ahrs_impl.imu_rate.r;
- const float qi = ahrs_impl.ltp_to_imu_quat.qi;
- const float qx = ahrs_impl.ltp_to_imu_quat.qx;
- const float qy = ahrs_impl.ltp_to_imu_quat.qy;
- const float qz = ahrs_impl.ltp_to_imu_quat.qz;
- ahrs_impl.ltp_to_imu_quat.qi = ca*qi - dp*qx - dq*qy - dr*qz;
- ahrs_impl.ltp_to_imu_quat.qx = dp*qi + ca*qx + dr*qy - dq*qz;
- ahrs_impl.ltp_to_imu_quat.qy = dq*qi - dr*qx + ca*qy + dp*qz;
- ahrs_impl.ltp_to_imu_quat.qz = dr*qi + dq*qx - dp*qy + ca*qz;
-
- // printf("%f\n", ahrs_impl.ltp_to_imu_quat.qi);
-
- FLOAT_RMAT_OF_QUAT(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat);
- }
+ /* propagate reference quaternion */
+ const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
+ FLOAT_QUAT_INTEGRATE(ahrs_impl.ltp_to_imu_quat, ahrs_impl.imu_rate, dt);
}
@@ -225,7 +204,7 @@ static inline void update_state(const struct FloatVect3 *i_expected, struct Floa
/* converted expected measurement from inertial to body frame */
struct FloatVect3 b_expected;
- FLOAT_RMAT_VECT3_MUL(b_expected, ahrs_impl.ltp_to_imu_rmat, *i_expected);
+ FLOAT_QUAT_VMULT(b_expected, ahrs_impl.ltp_to_imu_quat, *i_expected);
// S = HPH' + JRJ
float H[3][6] = {{ 0., -b_expected.z, b_expected.y, 0., 0., 0.},
@@ -285,7 +264,6 @@ static inline void reset_state(void) {
FLOAT_QUAT_NORMALIZE(q_tmp);
memcpy(&ahrs_impl.ltp_to_imu_quat, &q_tmp, sizeof(ahrs_impl.ltp_to_imu_quat));
FLOAT_QUAT_ZERO(ahrs_impl.gibbs_cor);
- FLOAT_RMAT_OF_QUAT(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat);
}
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h
index 3b5552fcad..043b519a64 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf.h
@@ -21,7 +21,7 @@
*/
/**
- * @file subsystems/ahrs/ahrs_float_mlkf_opt.h
+ * @file subsystems/ahrs/ahrs_float_mlkf.h
*
* Multiplicative linearized Kalman Filter in quaternion formulation.
*
@@ -37,20 +37,11 @@
struct AhrsMlkf {
struct FloatQuat ltp_to_imu_quat; ///< Rotation from LocalTangentPlane to IMU frame as unit quaternion
- struct FloatEulers ltp_to_imu_euler; ///< Rotation from LocalTangentPlane to IMU frame as Euler angles
- struct FloatRMat ltp_to_imu_rmat; ///< Rotation from LocalTangentPlane to IMU frame as Rotation Matrix
+ struct FloatQuat ltp_to_body_quat; ///< Rotation from LocalTangentPlane to body frame as unit quaternion
struct FloatRates imu_rate; ///< Rotational velocity in IMU frame
- struct FloatRates imu_rate_previous;
- struct FloatRates imu_rate_d;
-
- struct FloatQuat ltp_to_body_quat; ///< Rotation from LocalTangentPlane to body frame as unit quaternion
- struct FloatEulers ltp_to_body_euler; ///< Rotation from LocalTangentPlane to body frame as Euler angles
- struct FloatRMat ltp_to_body_rmat; ///< Rotation from LocalTangentPlane to body frame as Rotation Matrix
- struct FloatRates body_rate; ///< Rotational velocity in body frame
- struct FloatRates body_rate_d;
-
struct FloatRates gyro_bias;
+ struct FloatVect3 mag_h;
struct FloatQuat gibbs_cor;
float P[6][6];
diff --git a/sw/airborne/subsystems/ahrs/ahrs_gx3.c b/sw/airborne/subsystems/ahrs/ahrs_gx3.c
index 2911d64150..c6367005c9 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_gx3.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_gx3.c
@@ -30,9 +30,19 @@
* @author Michal Podhradsky
*/
#include "subsystems/ahrs/ahrs_gx3.h"
-#include "mcu_periph/sys_time.h"
-#define GX3_TIME(_ubx_payload) (uint32_t)((uint32_t)(*((uint8_t*)_ubx_payload+62+3))|(uint32_t)(*((uint8_t*)_ubx_payload+62+2))<<8|(uint32_t)(*((uint8_t*)_ubx_payload+62+1))<<16|(uint32_t)(*((uint8_t*)_ubx_payload+62+0))<<24)
+#ifdef AHRS_UPDATE_FW_ESTIMATOR
+// remotely settable
+#ifndef INS_ROLL_NEUTRAL_DEFAULT
+#define INS_ROLL_NEUTRAL_DEFAULT 0
+#endif
+#ifndef INS_PITCH_NEUTRAL_DEFAULT
+#define INS_PITCH_NEUTRAL_DEFAULT 0
+#endif
+float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
+float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
+#endif
+
#define GX3_CHKSM(_ubx_payload) (uint16_t)((uint16_t)(*((uint8_t*)_ubx_payload+66+1))|(uint16_t)(*((uint8_t*)_ubx_payload+66+0))<<8)
/*
@@ -41,24 +51,10 @@
* Positive roll : right wing down
* Positive yaw : clockwise
*/
-struct GX3_packet GX3_packet;
-enum GX3Status GX3_status;
-uint32_t GX3_time;
-uint32_t GX3_ltime;
-uint16_t GX3_chksm;
-uint16_t GX3_calcsm;
-float GX3_freq;
-
-struct FloatVect3 GX3_accel;
-struct FloatRates GX3_rate;
-struct FloatRMat GX3_rmat;
-struct FloatQuat GX3_quat;
-struct FloatEulers GX3_euler;
-
struct AhrsFloatQuat ahrs_impl;
struct AhrsAligner ahrs_aligner;
-static inline bool_t GX3_verify_chk(volatile uint8_t *buff_add);
+static inline bool_t gx3_verify_chk(volatile uint8_t *buff_add);
static inline float bef(volatile uint8_t *c);
/* Big Endian to Float */
@@ -73,7 +69,7 @@ static inline float bef(volatile uint8_t *c) {
return f;
}
-static inline bool_t GX3_verify_chk(volatile uint8_t *buff_add) {
+static inline bool_t gx3_verify_chk(volatile uint8_t *buff_add) {
uint16_t i,chk_calc;
chk_calc = 0;
for (i=0;i 180.0) {
+ course_f -= 360.0;
+ }
+ ltp_to_body_eulers.psi = (float)RadOfDeg(course_f);
+#endif
+ stateSetNedToBodyEulers_f(<p_to_body_eulers);
+#else
+#ifdef IMU_MAG_OFFSET //rotorcraft
+ struct FloatEulers ltp_to_body_eulers;
+ FLOAT_EULERS_OF_RMAT(ltp_to_body_eulers, ltp_to_body_rmat);
+ ltp_to_body_eulers.psi -= ahrs_impl.mag_offset;
+ stateSetNedToBodyEulers_f(<p_to_body_eulers);
+#else
+ stateSetNedToBodyRMat_f(<p_to_body_rmat);
+#endif
+#endif
}
/* GX3 Packet Collection */
-void GX3_packet_parse( uint8_t c ) {
- switch (GX3_packet.status) {
+void gx3_packet_parse( uint8_t c ) {
+ switch (ahrs_impl.gx3_packet.status) {
case GX3PacketWaiting:
- GX3_packet.msg_idx = 0;
+ ahrs_impl.gx3_packet.msg_idx = 0;
if (c == GX3_HEADER) {
- GX3_packet.status++;
- GX3_packet.msg_buf[GX3_packet.msg_idx] = c;
- GX3_packet.msg_idx++;
+ ahrs_impl.gx3_packet.status++;
+ ahrs_impl.gx3_packet.msg_buf[ahrs_impl.gx3_packet.msg_idx] = c;
+ ahrs_impl.gx3_packet.msg_idx++;
} else {
- GX3_packet.hdr_error++;
+ ahrs_impl.gx3_packet.hdr_error++;
}
break;
case GX3PacketReading:
- GX3_packet.msg_buf[GX3_packet.msg_idx] = c;
- GX3_packet.msg_idx++;
- if (GX3_packet.msg_idx == GX3_MSG_LEN) {
- if (GX3_verify_chk(GX3_packet.msg_buf)) {
- GX3_packet.msg_available = TRUE;
+ ahrs_impl.gx3_packet.msg_buf[ahrs_impl.gx3_packet.msg_idx] = c;
+ ahrs_impl.gx3_packet.msg_idx++;
+ if (ahrs_impl.gx3_packet.msg_idx == GX3_MSG_LEN) {
+ if (gx3_verify_chk(ahrs_impl.gx3_packet.msg_buf)) {
+ ahrs_impl.gx3_packet.msg_available = TRUE;
} else {
- GX3_packet.msg_available = FALSE;
- GX3_packet.chksm_error++;
+ ahrs_impl.gx3_packet.msg_available = FALSE;
+ ahrs_impl.gx3_packet.chksm_error++;
}
- GX3_packet.status = 0;
+ ahrs_impl.gx3_packet.status = 0;
}
break;
default:
- GX3_packet.status = 0;
- GX3_packet.msg_idx = 0;
+ ahrs_impl.gx3_packet.status = 0;
+ ahrs_impl.gx3_packet.msg_idx = 0;
break;
}
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
-
/* set ltp_to_imu so that body is zero */
QUAT_COPY(ahrs_impl.ltp_to_imu_quat, imuf.body_to_imu_quat);
- FLOAT_RATES_ZERO(ahrs_impl.imu_rate);
-
#ifdef IMU_MAG_OFFSET
ahrs_impl.mag_offset = IMU_MAG_OFFSET;
#else
- ahrs_impl.mag_offset = 0.;
+ ahrs_impl.mag_offset = 0.0;
#endif
-
ahrs_aligner.status = AHRS_ALIGNER_LOCKED;
}
void ahrs_aligner_run(void) {
#ifdef AHRS_ALIGNER_LED
- LED_TOGGLE(AHRS_ALIGNER_LED);
+ LED_ON(AHRS_ALIGNER_LED);
#endif
-
- if (GX3_freq > GX3_MIN_FREQ) {
- ahrs.status = AHRS_RUNNING;
-#ifdef AHRS_ALIGNER_LED
- LED_ON(AHRS_ALIGNER_LED);
-#endif
- }
+ ahrs.status = AHRS_RUNNING;
}
+
void ahrs_aligner_init(void) {
}
diff --git a/sw/airborne/subsystems/ahrs/ahrs_gx3.h b/sw/airborne/subsystems/ahrs/ahrs_gx3.h
index e56cf0ed37..cf2c464f21 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_gx3.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_gx3.h
@@ -36,18 +36,13 @@
#include "subsystems/imu.h"
#include "subsystems/ahrs.h"
#include "subsystems/ins.h"
+#include "subsystems/gps.h"
#include "mcu_periph/uart.h"
#include "subsystems/ahrs/ahrs_aligner.h"
#include "state.h"
#include "led.h"
-#define __GX3Link(dev, _x) dev##_x
-#define _GX3Link(dev, _x) __GX3Link(dev, _x)
-#define GX3Link(_x) _GX3Link(GX3_LINK, _x)
-
-#define GX3Buffer() GX3Link(ChAvailable())
-
#ifdef ImuScaleGyro
#undef ImuScaleGyro
#endif
@@ -70,18 +65,10 @@
#define IMU_GX3_LONG_DELAY 4000000
-extern struct GX3_packet GX3_packet;
-extern enum GX3Status GX3_status;
+extern void gx3_packet_read_message(void);
+extern void gx3_packet_parse(uint8_t c);
-extern void GX3_packet_read_message(void);
-extern void GX3_packet_parse(uint8_t c);
-
-extern float GX3_freq;
-extern uint16_t GX3_chksm;
-extern uint16_t GX3_calcsm;
-extern uint32_t GX3_time;
-
-struct GX3_packet {
+struct GX3Packet {
bool_t msg_available;
uint32_t chksm_error;
uint32_t hdr_error;
@@ -90,7 +77,7 @@ struct GX3_packet {
uint8_t msg_idx;
};
-enum GX36PacketStatus {
+enum GX3PacketStatus {
GX3PacketWaiting,
GX3PacketReading
};
@@ -102,30 +89,43 @@ enum GX3Status {
//AHRS
struct AhrsFloatQuat {
- struct FloatEulers ltp_to_imu_euler; ///< Rotation from LocalTangentPlane to IMU frame as Euler angles
struct FloatQuat ltp_to_imu_quat; ///< Rotation from LocalTangentPlane to IMU frame as quaternions
- struct FloatRates imu_rate; ///< Rotational velocity in IMU frame
- float mag_offset;
+ float mag_offset; ///< Difference between true and magnetic north
+
+ struct GX3Packet gx3_packet; ///< Packet struct
+ enum GX3Status gx3_status; ///< GX3 status
+ float gx3_freq; ///< data frequency
+ uint16_t gx3_chksm; ///< aux variable for checksum
+ uint32_t gx3_time; ///< GX3 time stamp
+ uint32_t gx3_ltime; ///< aux time stamp
+ struct FloatVect3 gx3_accel; ///< measured acceleration in IMU frame
+ struct FloatRates gx3_rate; ///< measured angular rates in IMU frame
+ struct FloatRMat gx3_rmat; ///< measured attitude in IMU frame (rotational matrix)
};
extern struct AhrsFloatQuat ahrs_impl;
static inline void ReadGX3Buffer(void) {
- while (GX3Link(ChAvailable()) && !GX3_packet.msg_available)
- GX3_packet_parse(GX3Link(Getch()));
+ while (uart_char_available(&GX3_PORT) && !ahrs_impl.gx3_packet.msg_available)
+ gx3_packet_parse(uart_getch(&GX3_PORT));
}
static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) {
- if (GX3Buffer()) {
+ if (uart_char_available(&GX3_PORT)) {
ReadGX3Buffer();
}
- if (GX3_packet.msg_available) {
- GX3_packet_read_message();
+ if (ahrs_impl.gx3_packet.msg_available) {
+ gx3_packet_read_message();
_gyro_handler();
_accel_handler();
_mag_handler();
- GX3_packet.msg_available = FALSE;
+ ahrs_impl.gx3_packet.msg_available = FALSE;
}
}
+#ifdef AHRS_UPDATE_FW_ESTIMATOR
+extern float ins_roll_neutral;
+extern float ins_pitch_neutral;
+#endif
+
#endif /* AHRS_GX3_H*/
diff --git a/sw/airborne/subsystems/datalink/datalink.h b/sw/airborne/subsystems/datalink/datalink.h
index 1259ba9ff1..ad1f8bb08c 100644
--- a/sw/airborne/subsystems/datalink/datalink.h
+++ b/sw/airborne/subsystems/datalink/datalink.h
@@ -46,6 +46,7 @@
#define PPRZ 1
#define XBEE 2
#define UDP 3
+#define SUPERBITRF 4
EXTERN bool_t dl_msg_available;
/** Flag provided to control calls to ::dl_parse_msg. NOT used in this module*/
@@ -90,10 +91,16 @@ EXTERN void dl_parse_msg(void);
#elif defined DATALINK && DATALINK == UDP
#define DatalinkEvent() { \
- UdpCheckAndParse(); \
+ UdpCheckAndParse(); \
DlCheckAndParse(); \
}
+#elif defined DATALINK && DATALINK == SUPERBITRF
+
+#define DatalinkEvent() { \
+ SuperbitRFCheckAndParse(); \
+ DlCheckAndParse(); \
+ }
#else
diff --git a/sw/airborne/subsystems/datalink/downlink.h b/sw/airborne/subsystems/datalink/downlink.h
index d8af126b68..f6da42088c 100644
--- a/sw/airborne/subsystems/datalink/downlink.h
+++ b/sw/airborne/subsystems/datalink/downlink.h
@@ -46,10 +46,14 @@
#endif
#else /** SITL */
+
#include "subsystems/datalink/udp.h"
#include "subsystems/datalink/pprz_transport.h"
#include "subsystems/datalink/xbee.h"
#include "subsystems/datalink/w5100.h"
+#if USE_SUPERBITRF
+#include "subsystems/datalink/superbitrf.h"
+#endif
#if USE_AUDIO_TELEMETRY
#include "subsystems/datalink/audio_telemetry.h"
#endif
@@ -57,6 +61,7 @@
#include "mcu_periph/usb_serial.h"
#endif
#include "mcu_periph/uart.h"
+
#endif /** !SITL */
#ifndef DefaultChannel
diff --git a/sw/airborne/subsystems/datalink/pprz_transport.h b/sw/airborne/subsystems/datalink/pprz_transport.h
index 10138a0726..502d54ebce 100644
--- a/sw/airborne/subsystems/datalink/pprz_transport.h
+++ b/sw/airborne/subsystems/datalink/pprz_transport.h
@@ -20,17 +20,22 @@
*
*/
-/** \file pprz_transport.h
- * \brief Building and parsing Paparazzi frames
+/**
+ * @file subsystems/datalink/pprz_transport.h
*
- * Pprz frame:
+ * Building and parsing Paparazzi frames.
*
- * |STX|length|... payload=(length-4) bytes ...|Checksum A|Checksum B|
+ * Pprz frame:
*
- * where checksum is computed over length and payload:
- * ck_A = ck_B = length
- * for each byte b in payload
- * ck_A += b; ck_b += ck_A
+ * |STX|length|... payload=(length-4) bytes ...|Checksum A|Checksum B|
+ *
+ * where checksum is computed over length and payload:
+ * @code
+ * ck_A = ck_B = length
+ * for each byte b in payload
+ * ck_A += b;
+ * ck_b += ck_A;
+ * @endcode
*/
#ifndef PPRZ_TRANSPORT_H
diff --git a/sw/airborne/subsystems/datalink/superbitrf.c b/sw/airborne/subsystems/datalink/superbitrf.c
new file mode 100644
index 0000000000..d4cf327d52
--- /dev/null
+++ b/sw/airborne/subsystems/datalink/superbitrf.c
@@ -0,0 +1,1004 @@
+/*
+ * Copyright (C) 2013 Freek van Tienen
+ *
+ * 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 subsystems/datalink/superbitrf.c
+ * DSM2 and DSMX datalink implementation for the cyrf6936 2.4GHz radio chip trough SPI
+ */
+
+#include "subsystems/datalink/superbitrf.h"
+
+#include
+#include "paparazzi.h"
+#include "mcu_periph/spi.h"
+#include "mcu_periph/sys_time.h"
+#include "mcu_periph/gpio.h"
+
+/* Default SuperbitRF SPI DEV */
+#ifndef SUPERBITRF_SPI_DEV
+#define SUPERBITRF_SPI_DEV spi1
+#endif
+PRINT_CONFIG_VAR(SUPERBITRF_SPI_DEV);
+
+/* Default SuperbitRF RST PORT and PIN */
+#ifndef SUPERBITRF_RST_PORT
+#define SUPERBITRF_RST_PORT GPIOC
+#endif
+PRINT_CONFIG_VAR(SUPERBITRF_RST_PORT);
+#ifndef SUPERBITRF_RST_PIN
+#define SUPERBITRF_RST_PIN GPIO12
+#endif
+PRINT_CONFIG_VAR(SUPERBITRF_RST_PIN);
+
+/* Default SuperbitRF DRDY(IRQ) PORT and PIN */
+#ifndef SUPERBITRF_DRDY_PORT
+#define SUPERBITRF_DRDY_PORT GPIOB
+#endif
+PRINT_CONFIG_VAR(SUPERBITRF_DRDY_PORT);
+#ifndef SUPERBITRF_DRDY_PIN
+#define SUPERBITRF_DRDY_PIN GPIO1
+#endif
+PRINT_CONFIG_VAR(SUPERBITRF_DRDY_PIN);
+
+/* Default forcing in DSM2 mode is false */
+#ifndef SUPERBITRF_FORCE_DSM2
+#define SUPERBITRF_FORCE_DSM2 TRUE
+#endif
+PRINT_CONFIG_VAR(SUPERBITRF_FORCE_DSM2);
+
+/* The superbitRF structure */
+struct SuperbitRF superbitrf;
+
+/* The internal functions */
+static inline void superbitrf_radio_to_channels(uint8_t* data, uint8_t nb_channels, bool_t is_11bit, int16_t* channels);
+static inline void superbitrf_receive_packet_cb(bool_t error, uint8_t status, uint8_t packet[]);
+static inline void superbitrf_send_packet_cb(bool_t error);
+static inline void superbitrf_gen_dsmx_channels(void);
+
+/* The startup configuration for the cyrf6936 */
+static const uint8_t cyrf_stratup_config[][2] = {
+ {CYRF_MODE_OVERRIDE, CYRF_RST}, // Reset the device
+ {CYRF_CLK_EN, CYRF_RXF}, // Enable the clock
+ {CYRF_AUTO_CAL_TIME, 0x3C}, // From manual, needed for initialization
+ {CYRF_AUTO_CAL_OFFSET, 0x14}, // From manual, needed for initialization
+ {CYRF_RX_CFG, CYRF_LNA | CYRF_FAST_TURN_EN}, // Enable low noise amplifier and fast turning
+ {CYRF_TX_OFFSET_LSB, 0x55}, // From manual, typical configuration
+ {CYRF_TX_OFFSET_MSB, 0x05}, // From manual, typical configuration
+ {CYRF_XACT_CFG, CYRF_MODE_SYNTH_RX | CYRF_FRC_END}, // Force in Synth RX mode
+ {CYRF_TX_CFG, CYRF_DATA_CODE_LENGTH | CYRF_DATA_MODE_SDR | CYRF_PA_4}, // Enable 64 chip codes, SDR mode and amplifier +4dBm
+ {CYRF_DATA64_THOLD, 0x0E}, // From manual, typical configuration
+ {CYRF_XACT_CFG, CYRF_MODE_SYNTH_RX}, // Set in Synth RX mode (again, really needed?)
+};
+/* The binding configuration for the cyrf6936 */
+static const uint8_t cyrf_bind_config[][2] = {
+ {CYRF_TX_CFG, CYRF_DATA_CODE_LENGTH | CYRF_DATA_MODE_SDR | CYRF_PA_4}, // Enable 64 chip codes, SDR mode and amplifier +4dBm
+ {CYRF_FRAMING_CFG, CYRF_SOP_LEN | 0xE}, // Set SOP CODE to 64 chips and SOP Correlator Threshold to 0xE
+ {CYRF_RX_OVERRIDE, CYRF_FRC_RXDR | CYRF_DIS_RXCRC}, // Force receive data rate and disable receive CRC checker
+ {CYRF_EOP_CTRL, 0x02}, // Only enable EOP symbol count of 2
+ {CYRF_TX_OVERRIDE, CYRF_DIS_TXCRC}, // Disable transmit CRC generate
+};
+/* The transfer configuration for the cyrf6936 */
+static const uint8_t cyrf_transfer_config[][2] = {
+ {CYRF_TX_CFG, CYRF_DATA_CODE_LENGTH | CYRF_DATA_MODE_8DR | CYRF_PA_4}, // Enable 64 chip codes, 8DR mode and amplifier +4dBm
+ {CYRF_FRAMING_CFG, CYRF_SOP_EN | CYRF_SOP_LEN | CYRF_LEN_EN | 0xE}, // Set SOP CODE enable, SOP CODE to 64 chips, Packet length enable, and SOP Correlator Threshold to 0xE
+ {CYRF_TX_OVERRIDE, 0x00}, // Reset TX overrides
+ {CYRF_RX_OVERRIDE, 0x00}, // Reset RX overrides
+};
+/* Abort the receive of the cyrf6936 */
+const uint8_t cyrf_abort_receive[][2] = {
+ {CYRF_XACT_CFG, CYRF_MODE_SYNTH_RX | CYRF_FRC_END},
+ {CYRF_RX_ABORT, 0x00}
+};
+/* Start the receive of the cyrf6936 */
+const uint8_t cyrf_start_receive[][2] = {
+ {CYRF_RX_IRQ_STATUS, CYRF_RXOW_IRQ}, // Clear the IRQ
+ {CYRF_RX_CTRL, CYRF_RX_GO | CYRF_RXC_IRQEN | CYRF_RXE_IRQEN} // Start receiving and set the IRQ
+};
+
+/* The PN codes used for DSM2 and DSMX channel hopping */
+static const uint8_t pn_codes[5][9][8] = {
+{ /* Row 0 */
+ /* Col 0 */ {0x03, 0xBC, 0x6E, 0x8A, 0xEF, 0xBD, 0xFE, 0xF8},
+ /* Col 1 */ {0x88, 0x17, 0x13, 0x3B, 0x2D, 0xBF, 0x06, 0xD6},
+ /* Col 2 */ {0xF1, 0x94, 0x30, 0x21, 0xA1, 0x1C, 0x88, 0xA9},
+ /* Col 3 */ {0xD0, 0xD2, 0x8E, 0xBC, 0x82, 0x2F, 0xE3, 0xB4},
+ /* Col 4 */ {0x8C, 0xFA, 0x47, 0x9B, 0x83, 0xA5, 0x66, 0xD0},
+ /* Col 5 */ {0x07, 0xBD, 0x9F, 0x26, 0xC8, 0x31, 0x0F, 0xB8},
+ /* Col 6 */ {0xEF, 0x03, 0x95, 0x89, 0xB4, 0x71, 0x61, 0x9D},
+ /* Col 7 */ {0x40, 0xBA, 0x97, 0xD5, 0x86, 0x4F, 0xCC, 0xD1},
+ /* Col 8 */ {0xD7, 0xA1, 0x54, 0xB1, 0x5E, 0x89, 0xAE, 0x86}
+},
+{ /* Row 1 */
+ /* Col 0 */ {0x83, 0xF7, 0xA8, 0x2D, 0x7A, 0x44, 0x64, 0xD3},
+ /* Col 1 */ {0x3F, 0x2C, 0x4E, 0xAA, 0x71, 0x48, 0x7A, 0xC9},
+ /* Col 2 */ {0x17, 0xFF, 0x9E, 0x21, 0x36, 0x90, 0xC7, 0x82},
+ /* Col 3 */ {0xBC, 0x5D, 0x9A, 0x5B, 0xEE, 0x7F, 0x42, 0xEB},
+ /* Col 4 */ {0x24, 0xF5, 0xDD, 0xF8, 0x7A, 0x77, 0x74, 0xE7},
+ /* Col 5 */ {0x3D, 0x70, 0x7C, 0x94, 0xDC, 0x84, 0xAD, 0x95},
+ /* Col 6 */ {0x1E, 0x6A, 0xF0, 0x37, 0x52, 0x7B, 0x11, 0xD4},
+ /* Col 7 */ {0x62, 0xF5, 0x2B, 0xAA, 0xFC, 0x33, 0xBF, 0xAF},
+ /* Col 8 */ {0x40, 0x56, 0x32, 0xD9, 0x0F, 0xD9, 0x5D, 0x97}
+},
+{ /* Row 2 */
+ /* Col 0 */ {0x40, 0x56, 0x32, 0xD9, 0x0F, 0xD9, 0x5D, 0x97},
+ /* Col 1 */ {0x8E, 0x4A, 0xD0, 0xA9, 0xA7, 0xFF, 0x20, 0xCA},
+ /* Col 2 */ {0x4C, 0x97, 0x9D, 0xBF, 0xB8, 0x3D, 0xB5, 0xBE},
+ /* Col 3 */ {0x0C, 0x5D, 0x24, 0x30, 0x9F, 0xCA, 0x6D, 0xBD},
+ /* Col 4 */ {0x50, 0x14, 0x33, 0xDE, 0xF1, 0x78, 0x95, 0xAD},
+ /* Col 5 */ {0x0C, 0x3C, 0xFA, 0xF9, 0xF0, 0xF2, 0x10, 0xC9},
+ /* Col 6 */ {0xF4, 0xDA, 0x06, 0xDB, 0xBF, 0x4E, 0x6F, 0xB3},
+ /* Col 7 */ {0x9E, 0x08, 0xD1, 0xAE, 0x59, 0x5E, 0xE8, 0xF0},
+ /* Col 8 */ {0xC0, 0x90, 0x8F, 0xBB, 0x7C, 0x8E, 0x2B, 0x8E}
+},
+{ /* Row 3 */
+ /* Col 0 */ {0xC0, 0x90, 0x8F, 0xBB, 0x7C, 0x8E, 0x2B, 0x8E},
+ /* Col 1 */ {0x80, 0x69, 0x26, 0x80, 0x08, 0xF8, 0x49, 0xE7},
+ /* Col 2 */ {0x7D, 0x2D, 0x49, 0x54, 0xD0, 0x80, 0x40, 0xC1},
+ /* Col 3 */ {0xB6, 0xF2, 0xE6, 0x1B, 0x80, 0x5A, 0x36, 0xB4},
+ /* Col 4 */ {0x42, 0xAE, 0x9C, 0x1C, 0xDA, 0x67, 0x05, 0xF6},
+ /* Col 5 */ {0x9B, 0x75, 0xF7, 0xE0, 0x14, 0x8D, 0xB5, 0x80},
+ /* Col 6 */ {0xBF, 0x54, 0x98, 0xB9, 0xB7, 0x30, 0x5A, 0x88},
+ /* Col 7 */ {0x35, 0xD1, 0xFC, 0x97, 0x23, 0xD4, 0xC9, 0x88},
+ /* Col 8 */ {0x88, 0xE1, 0xD6, 0x31, 0x26, 0x5F, 0xBD, 0x40}
+},
+{ /* Row 4 */
+ /* Col 0 */ {0xE1, 0xD6, 0x31, 0x26, 0x5F, 0xBD, 0x40, 0x93},
+ /* Col 1 */ {0xDC, 0x68, 0x08, 0x99, 0x97, 0xAE, 0xAF, 0x8C},
+ /* Col 2 */ {0xC3, 0x0E, 0x01, 0x16, 0x0E, 0x32, 0x06, 0xBA},
+ /* Col 3 */ {0xE0, 0x83, 0x01, 0xFA, 0xAB, 0x3E, 0x8F, 0xAC},
+ /* Col 4 */ {0x5C, 0xD5, 0x9C, 0xB8, 0x46, 0x9C, 0x7D, 0x84},
+ /* Col 5 */ {0xF1, 0xC6, 0xFE, 0x5C, 0x9D, 0xA5, 0x4F, 0xB7},
+ /* Col 6 */ {0x58, 0xB5, 0xB3, 0xDD, 0x0E, 0x28, 0xF1, 0xB0},
+ /* Col 7 */ {0x5F, 0x30, 0x3B, 0x56, 0x96, 0x45, 0xF4, 0xA1},
+ /* Col 8 */ {0x03, 0xBC, 0x6E, 0x8A, 0xEF, 0xBD, 0xFE, 0xF8}
+},
+};
+static const uint8_t pn_bind[] = { 0x98, 0x88, 0x1B, 0xE4, 0x30, 0x79, 0x03, 0x84 };
+
+#if DOWNLINK
+#include "subsystems/datalink/telemetry.h"
+
+static void send_superbit(void) {
+ DOWNLINK_SEND_SUPERBITRF(DefaultChannel, DefaultDevice,
+ &superbitrf.status,
+ &superbitrf.cyrf6936.status,
+ &superbitrf.irq_count,
+ &superbitrf.rx_packet_count,
+ &superbitrf.tx_packet_count,
+ &superbitrf.transfer_timeouts,
+ &superbitrf.resync_count,
+ &superbitrf.uplink_count,
+ &superbitrf.rc_count,
+ &superbitrf.timing1,
+ &superbitrf.timing2,
+ &superbitrf.bind_mfg_id32,
+ 6,
+ superbitrf.cyrf6936.mfg_id);
+}
+#endif
+
+/**
+ * Initialize the superbitrf
+ */
+void superbitrf_init(void) {
+ // Set the status to uninitialized and set the timer to 0
+ superbitrf.status = SUPERBITRF_UNINIT;
+ superbitrf.state = 0;
+ superbitrf.timer = 0;
+ superbitrf.rx_packet_count = 0;
+ superbitrf.tx_packet_count = 0;
+
+ // Setup the transmit buffer
+ superbitrf.tx_insert_idx = 0;
+ superbitrf.tx_extract_idx = 0;
+
+ // Initialize the binding pin
+ gpio_setup_input(SPEKTRUM_BIND_PIN_PORT, SPEKTRUM_BIND_PIN);
+
+ // Initialize the IRQ/DRDY pin
+ gpio_setup_input(SUPERBITRF_DRDY_PORT, SUPERBITRF_DRDY_PIN);
+
+ // Initialize the cyrf6936 chip
+ cyrf6936_init(&superbitrf.cyrf6936, &(SUPERBITRF_SPI_DEV), 2, SUPERBITRF_RST_PORT, SUPERBITRF_RST_PIN);
+
+#if DOWNLINK
+ register_periodic_telemetry(DefaultPeriodic, "SUPERBIT", send_superbit);
+#endif
+}
+
+/**
+ * The superbitrf on event call
+ */
+void superbitrf_event(void) {
+ uint8_t i, pn_row, data_code[16];
+ static uint8_t packet_size, tx_packet[16];
+ static bool_t start_transfer = TRUE;
+
+ // Check if the cyrf6936 isn't busy and the uperbitrf is initialized
+ if(superbitrf.cyrf6936.status != CYRF6936_IDLE)
+ return;
+
+ // When the device is initialized handle the IRQ
+ if(superbitrf.status != SUPERBITRF_UNINIT) {
+ // First handle the IRQ
+ if(gpio_get(SUPERBITRF_DRDY_PORT, SUPERBITRF_DRDY_PIN) == 0) {
+ // Receive the packet
+ cyrf6936_read_rx_irq_status_packet(&superbitrf.cyrf6936);
+ superbitrf.irq_count++;
+ }
+
+ /* Check if it is a valid receive */
+ if(superbitrf.cyrf6936.has_irq && (superbitrf.cyrf6936.rx_irq_status & CYRF_RXC_IRQ)) {
+ // Handle the packet received
+ superbitrf_receive_packet_cb((superbitrf.cyrf6936.rx_irq_status & CYRF_RXE_IRQ), superbitrf.cyrf6936.rx_status, superbitrf.cyrf6936.rx_packet);
+ superbitrf.rx_packet_count++;
+
+ // Reset the packet receiving
+ superbitrf.cyrf6936.has_irq = FALSE;
+ }
+
+ /* Check if it has a valid send */
+ if(superbitrf.cyrf6936.has_irq && (superbitrf.cyrf6936.tx_irq_status & CYRF_TXC_IRQ)) {
+ // Handle the send packet
+ superbitrf_send_packet_cb((superbitrf.cyrf6936.rx_irq_status & CYRF_TXE_IRQ));
+ superbitrf.tx_packet_count++;
+
+ // Reset the packet receiving
+ superbitrf.cyrf6936.has_irq = FALSE;
+ }
+ }
+
+ // Check the status of the superbitrf
+ switch(superbitrf.status) {
+
+ /* When the superbitrf isn't initialized */
+ case SUPERBITRF_UNINIT:
+ // Try to write the startup config
+ if(cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_stratup_config, 11)) {
+ // Check if need to go to bind or transfer
+ if(gpio_get(SPEKTRUM_BIND_PIN_PORT, SPEKTRUM_BIND_PIN) == 0)
+ start_transfer = FALSE;
+
+ superbitrf.status = SUPERBITRF_INIT_BINDING;
+ }
+ break;
+
+ /* When the superbitrf is initializing binding */
+ case SUPERBITRF_INIT_BINDING:
+ /* Switch the different states */
+ switch (superbitrf.state) {
+ case 0:
+ // Try to write the binding config
+ cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_bind_config, 5);
+ superbitrf.state++;
+ break;
+ case 1:
+ // Set the data code and channel
+ memcpy(data_code, pn_codes[0][8], 8);
+ memcpy(data_code + 8, pn_bind, 8);
+ cyrf6936_write_chan_sop_data_crc(&superbitrf.cyrf6936, 1, pn_codes[0][0], data_code, 0x0000);
+
+ superbitrf.status = SUPERBITRF_BINDING;
+ break;
+ default:
+ // Should not happen
+ superbitrf.state = 0;
+ break;
+ }
+ break;
+
+ /* When the superbitrf is initializing transfer */
+ case SUPERBITRF_INIT_TRANSFER:
+ // Generate the DSMX channels
+ superbitrf_gen_dsmx_channels();
+
+ // Try to write the transfer config
+ if(cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_transfer_config, 4)) {
+ superbitrf.resync_count = 0;
+ superbitrf.packet_loss = FALSE;
+ superbitrf.packet_loss_bit = 0;
+ superbitrf.status = SUPERBITRF_SYNCING_A;
+ superbitrf.state = 1;
+ }
+ break;
+
+ /* When the superbitrf is in binding mode */
+ case SUPERBITRF_BINDING:
+ /* Switch the different states */
+ switch (superbitrf.state) {
+ case 0:
+ // When there is a timeout
+ if (superbitrf.timer < get_sys_time_usec())
+ superbitrf.state++;
+ break;
+ case 1:
+ // Abort the receive
+ cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_abort_receive, 2);
+
+ superbitrf.state++;
+ break;
+ case 2:
+ // Switch channel
+ superbitrf.channel = (superbitrf.channel + 2) % 0x4F; //TODO fix define
+ cyrf6936_write(&superbitrf.cyrf6936, CYRF_CHANNEL, superbitrf.channel);
+
+ superbitrf.state += 2; // Already aborted
+ break;
+ case 3:
+ // Abort the receive
+ cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_abort_receive, 2);
+
+ superbitrf.state++;
+ break;
+ case 4:
+ // Start receiving
+ cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2);
+ superbitrf.state++;
+ break;
+ default:
+ // Check if need to go to transfer
+ if(start_transfer) {
+ // Initialize the binding values
+ #ifdef RADIO_TRANSMITTER_ID
+ PRINT_CONFIG_VAR(RADIO_TRANSMITTER_ID);
+ superbitrf.bind_mfg_id32 = RADIO_TRANSMITTER_ID;
+ superbitrf.bind_mfg_id[0] = (superbitrf.bind_mfg_id32 &0xFF);
+ superbitrf.bind_mfg_id[1] = (superbitrf.bind_mfg_id32 >>8 &0xFF);
+ superbitrf.bind_mfg_id[2] = (superbitrf.bind_mfg_id32 >>16 &0xFF);
+ superbitrf.bind_mfg_id[3] = (superbitrf.bind_mfg_id32 >>24 &0xFF);
+
+ // Calculate some values based on the bind MFG id
+ superbitrf.crc_seed = ~((superbitrf.bind_mfg_id[0] << 8) + superbitrf.bind_mfg_id[1]);
+ superbitrf.sop_col = (superbitrf.bind_mfg_id[0] + superbitrf.bind_mfg_id[1] + superbitrf.bind_mfg_id[2] + 2) & 0x07;
+ superbitrf.data_col = 7 - superbitrf.sop_col;
+ #endif
+ #ifdef RADIO_TRANSMITTER_CHAN
+ PRINT_CONFIG_VAR(RADIO_TRANSMITTER_CHAN);
+ superbitrf.num_channels = RADIO_TRANSMITTER_CHAN;
+ #endif
+ #ifdef RADIO_TRANSMITTER_PROTOCOL
+ PRINT_CONFIG_VAR(RADIO_TRANSMITTER_PROTOCOL);
+ superbitrf.protocol = RADIO_TRANSMITTER_PROTOCOL;
+ superbitrf.resolution = (superbitrf.protocol & 0x10)>>4;
+ #endif
+
+ // Start transfer
+ superbitrf.state = 0;
+ superbitrf.status = SUPERBITRF_INIT_TRANSFER;
+ break;
+ }
+
+ // Set the timer
+ superbitrf.timer = (get_sys_time_usec() + SUPERBITRF_BIND_RECV_TIME) % 0xFFFFFFFF;
+ superbitrf.state = 0;
+ break;
+ }
+ break;
+
+ /* When the receiver is synchronizing with the transmitter */
+ case SUPERBITRF_SYNCING_A:
+ case SUPERBITRF_SYNCING_B:
+ /* Switch the different states */
+ switch (superbitrf.state) {
+ case 0:
+ // When there is a timeout
+ if (superbitrf.timer < get_sys_time_usec())
+ superbitrf.state++;
+ break;
+ case 1:
+ // Abort the receive
+ cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_abort_receive, 2);
+ superbitrf.state++;
+ break;
+ case 2:
+ // Switch channel, sop code, data code and crc
+ superbitrf.channel_idx = (IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2)? (superbitrf.channel_idx + 1) %2 : (superbitrf.channel_idx + 1) %23;
+ pn_row = (IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2)? superbitrf.channels[superbitrf.channel_idx] % 5 : (superbitrf.channels[superbitrf.channel_idx]-2) % 5;
+
+ cyrf6936_write_chan_sop_data_crc(&superbitrf.cyrf6936, superbitrf.channels[superbitrf.channel_idx],
+ pn_codes[pn_row][superbitrf.sop_col],
+ pn_codes[pn_row][superbitrf.data_col],
+ superbitrf.crc_seed);
+ superbitrf.state++;
+ break;
+ case 3:
+ // Create a new packet when no packet loss
+ if(!superbitrf.packet_loss) {
+ superbitrf.packet_loss_bit = !superbitrf.packet_loss_bit;
+ if(IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) {
+ tx_packet[0] = ~superbitrf.bind_mfg_id[2];
+ tx_packet[1] = (~superbitrf.bind_mfg_id[3])+1+superbitrf.packet_loss_bit;
+ } else {
+ tx_packet[0] = superbitrf.bind_mfg_id[2];
+ tx_packet[1] = (superbitrf.bind_mfg_id[3])+1+superbitrf.packet_loss_bit;
+ }
+
+ packet_size = (superbitrf.tx_insert_idx-superbitrf.tx_extract_idx+128 %128);
+ if(packet_size > 14)
+ packet_size = 14;
+
+ for(i = 0; i < packet_size; i++)
+ tx_packet[i+2] = superbitrf.tx_buffer[(superbitrf.tx_extract_idx+i) %128];
+ }
+
+ // Send a packet
+ cyrf6936_send(&superbitrf.cyrf6936, tx_packet, packet_size+2);
+
+ // Update the packet extraction
+ if(!superbitrf.packet_loss)
+ superbitrf.tx_extract_idx = (superbitrf.tx_extract_idx+packet_size) %128;
+
+ superbitrf.state++;
+ break;
+ case 4:
+ //TODO: check timeout? (Waiting for send)
+ break;
+ case 5:
+ superbitrf.state = 7;
+ break;
+ // Start receiving
+ cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2);
+ superbitrf.timer = (get_sys_time_usec() + SUPERBITRF_DATARECVB_TIME) % 0xFFFFFFFF;
+ superbitrf.state++;
+ break;
+ case 6:
+ // Wait for telemetry data
+ if (superbitrf.timer < get_sys_time_usec())
+ superbitrf.state++;
+ break;
+ case 7:
+ // When DSMX we don't need to switch
+ if(IS_DSMX(superbitrf.protocol) && !SUPERBITRF_FORCE_DSM2) {
+ superbitrf.state++;
+ superbitrf.channel = superbitrf.channels[superbitrf.channel_idx];
+ break;
+ }
+
+ // Switch channel, sop code, data code and crc
+ superbitrf.channel = (superbitrf.channel + 2) % 0x4F; //TODO fix define
+ superbitrf.crc_seed = ~superbitrf.crc_seed;
+ pn_row = (IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2)? superbitrf.channel % 5 : (superbitrf.channel-2) % 5;
+
+ cyrf6936_write_chan_sop_data_crc(&superbitrf.cyrf6936, superbitrf.channel,
+ pn_codes[pn_row][superbitrf.sop_col],
+ pn_codes[pn_row][superbitrf.data_col],
+ superbitrf.crc_seed);
+
+ superbitrf.state++;
+ break;
+ case 8:
+ // Start receiving
+ cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2);
+ superbitrf.state++;
+ break;
+ default:
+ // Set the timer
+ superbitrf.timer = (get_sys_time_usec() + SUPERBITRF_SYNC_RECV_TIME) % 0xFFFFFFFF;
+ superbitrf.state = 0;
+ break;
+ }
+ break;
+
+ /* Normal transfer mode */
+ case SUPERBITRF_TRANSFER:
+ /* Switch the different states */
+ switch (superbitrf.state) {
+ case 0:
+ // Fixing timer overflow
+ if(superbitrf.timer_overflow && get_sys_time_usec() <= superbitrf.timer)
+ superbitrf.timer_overflow = FALSE;
+
+ // When there is a timeout
+ if(superbitrf.timer < get_sys_time_usec() && !superbitrf.timer_overflow) {
+ superbitrf.transfer_timeouts++;
+ superbitrf.timeouts++;
+ superbitrf.state++;
+ }
+
+ // We really lost the communication
+ if(superbitrf.timeouts > 100) {
+ superbitrf.state = 0;
+ superbitrf.resync_count++;
+ superbitrf.status = SUPERBITRF_SYNCING_A;
+ }
+ break;
+ case 1:
+ // Abort the receive
+ cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_abort_receive, 2);
+ superbitrf.state++;
+
+ // Set the timer
+ superbitrf.timer = (get_sys_time_usec() + SUPERBITRF_DATARECV_TIME) % 0xFFFFFFFF;
+ if(superbitrf.timer < get_sys_time_usec())
+ superbitrf.timer_overflow = TRUE;
+ else
+ superbitrf.timer_overflow = FALSE;
+
+ // Only send on channel 2
+ if(superbitrf.crc_seed != ((superbitrf.bind_mfg_id[0] << 8) + superbitrf.bind_mfg_id[1]))
+ superbitrf.state = 8;
+ break;
+ case 2:
+ // Wait before sending (FIXME??)
+ superbitrf.state++;
+ break;
+ case 3:
+ // Create a new packet when no packet loss
+ if(!superbitrf.packet_loss) {
+ superbitrf.packet_loss_bit = !superbitrf.packet_loss_bit;
+ if(IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) {
+ tx_packet[0] = ~superbitrf.bind_mfg_id[2];
+ tx_packet[1] = ((~superbitrf.bind_mfg_id[3])+1+superbitrf.packet_loss_bit) % 0xFF;
+ } else {
+ tx_packet[0] = superbitrf.bind_mfg_id[2];
+ tx_packet[1] = ((superbitrf.bind_mfg_id[3])+1+superbitrf.packet_loss_bit) % 0xFF;
+ }
+
+ packet_size = (superbitrf.tx_insert_idx-superbitrf.tx_extract_idx+128 %128);
+ if(packet_size > 14)
+ packet_size = 14;
+
+ for(i = 0; i < packet_size; i++)
+ tx_packet[i+2] = superbitrf.tx_buffer[(superbitrf.tx_extract_idx+i) %128];
+ }
+
+ // Send a packet
+ cyrf6936_send(&superbitrf.cyrf6936, tx_packet, packet_size+2);
+
+ // Update the packet extraction
+ if(!superbitrf.packet_loss)
+ superbitrf.tx_extract_idx = (superbitrf.tx_extract_idx+packet_size) %128;
+
+ superbitrf.state++;
+ break;
+ case 4:
+ //TODO: check timeout? (Waiting for send)
+ break;
+ case 5:
+ // Start receiving
+ cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2);
+ superbitrf.state++;
+ break;
+ case 6:
+ // Fixing timer overflow
+ if(superbitrf.timer_overflow && get_sys_time_usec() <= superbitrf.timer)
+ superbitrf.timer_overflow = FALSE;
+
+ // Waiting for data receive
+ if (superbitrf.timer < get_sys_time_usec() && !superbitrf.timer_overflow)
+ superbitrf.state++;
+ break;
+ case 7:
+ // Abort the receive
+ cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_abort_receive, 2);
+ superbitrf.state++;
+ break;
+ case 8:
+ // Switch channel, sop code, data code and crc
+ superbitrf.channel_idx = (IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2)? (superbitrf.channel_idx + 1) %2 : (superbitrf.channel_idx + 1) %23;
+ superbitrf.channel = superbitrf.channels[superbitrf.channel_idx];
+ superbitrf.crc_seed = ~superbitrf.crc_seed;
+ pn_row = (IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2)? superbitrf.channel % 5 : (superbitrf.channel-2) % 5;
+
+ cyrf6936_write_chan_sop_data_crc(&superbitrf.cyrf6936, superbitrf.channel,
+ pn_codes[pn_row][superbitrf.sop_col],
+ pn_codes[pn_row][superbitrf.data_col],
+ superbitrf.crc_seed);
+
+ superbitrf.state++;
+ break;
+ case 9:
+ // Start receiving
+ cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2);
+ superbitrf.state++;
+ break;
+ default:
+ // Set the timer
+ if(superbitrf.crc_seed != ((superbitrf.bind_mfg_id[0] << 8) + superbitrf.bind_mfg_id[1]))
+ superbitrf.timer = (superbitrf.timer - SUPERBITRF_DATARECV_TIME + SUPERBITRF_RECV_TIME) % 0xFFFFFFFF;
+ else
+ superbitrf.timer = (superbitrf.timer - SUPERBITRF_DATARECV_TIME + SUPERBITRF_RECV_SHORT_TIME) % 0xFFFFFFFF;
+ if(superbitrf.timer < get_sys_time_usec())
+ superbitrf.timer_overflow = TRUE;
+ else
+ superbitrf.timer_overflow = FALSE;
+
+ superbitrf.state = 0;
+ break;
+ }
+ break;
+
+ /* Should not come here */
+ default:
+ break;
+ }
+}
+
+/**
+ * When we receive a packet this callback is called
+ */
+static inline void superbitrf_receive_packet_cb(bool_t error, uint8_t status, uint8_t packet[]) {
+ int i;
+ uint16_t sum;
+
+ /* Switch on the status of the superbitRF */
+ switch (superbitrf.status) {
+
+ /* When we are in binding mode */
+ case SUPERBITRF_BINDING:
+ // Check if the MFG id is exactly the same
+ if (packet[0] != packet[4] || packet[1] != packet[5] || packet[2] != packet[6] || packet[3] != packet[7]) {
+ // Start receiving without changing channel
+ superbitrf.state = 3;
+ break;
+ }
+
+ // Calculate the first sum
+ sum = 384 - 0x10;
+ for (i = 0; i < 8; i++)
+ sum += packet[i];
+
+ // Check the first sum
+ if (packet[8] != sum >> 8 || packet[9] != (sum & 0xFF)) {
+ // Start receiving without changing channel
+ superbitrf.state = 3;
+ break;
+ }
+
+ // Calculate second sum
+ for (i = 8; i < 14; i++)
+ sum += packet[i];
+
+ // Check the second sum
+ if (packet[14] != sum >> 8 || packet[15] != (sum & 0xFF)) {
+ // Start receiving without changing channel
+ superbitrf.state = 3;
+ break;
+ }
+
+ // Update the mfg id, number of channels and protocol
+ superbitrf.bind_mfg_id[0] = ~packet[0];
+ superbitrf.bind_mfg_id[1] = ~packet[1];
+ superbitrf.bind_mfg_id[2] = ~packet[2];
+ superbitrf.bind_mfg_id[3] = ~packet[3];
+ superbitrf.bind_mfg_id32 = ((superbitrf.bind_mfg_id[3] &0xFF) << 24 | (superbitrf.bind_mfg_id[2] &0xFF) << 16 |
+ (superbitrf.bind_mfg_id[1] &0xFF) << 8 | (superbitrf.bind_mfg_id[0] &0xFF));
+ superbitrf.num_channels = packet[11];
+ superbitrf.protocol = packet[12];
+ superbitrf.resolution = (superbitrf.protocol & 0x10)>>4;
+
+ // Calculate some values based on the bind MFG id
+ superbitrf.crc_seed = ~((superbitrf.bind_mfg_id[0] << 8) + superbitrf.bind_mfg_id[1]);
+ superbitrf.sop_col = (superbitrf.bind_mfg_id[0] + superbitrf.bind_mfg_id[1] + superbitrf.bind_mfg_id[2] + 2) & 0x07;
+ superbitrf.data_col = 7 - superbitrf.sop_col;
+
+ // Update the status of the receiver
+ superbitrf.state = 0;
+ superbitrf.status = SUPERBITRF_INIT_TRANSFER;
+ break;
+
+ /* When we receive a packet during syncing first channel A */
+ case SUPERBITRF_SYNCING_A:
+ // Check the MFG id
+ if(error && !(status & CYRF_BAD_CRC)) {
+ // Start receiving TODO: Fix nicely
+ cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2);
+ break;
+ }
+ if((IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) &&
+ (packet[0] != (~superbitrf.bind_mfg_id[2]&0xFF) || (packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF) &&
+ packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF)+1))) {
+ // Start receiving TODO: Fix nicely
+ cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2);
+ break;
+ }
+ if((IS_DSMX(superbitrf.protocol) && !SUPERBITRF_FORCE_DSM2) &&
+ (packet[0] != (superbitrf.bind_mfg_id[2]&0xFF) || (packet[1] != (superbitrf.bind_mfg_id[3]&0xFF) &&
+ packet[1] != (superbitrf.bind_mfg_id[3]&0xFF)+1))) {
+ // Start receiving TODO: Fix nicely
+ cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2);
+ break;
+ }
+
+ // If the CRC is wrong invert
+ if (error && (status & CYRF_BAD_CRC))
+ superbitrf.crc_seed = ~superbitrf.crc_seed;
+
+ // When we receive a data packet
+ if(packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF) && packet[1] != (superbitrf.bind_mfg_id[3]&0xFF)) {
+ superbitrf.uplink_count++;
+
+ // Check if it is a data loss packet
+ if(packet[1] != (~superbitrf.bind_mfg_id[3] + 1 + superbitrf.packet_loss_bit)%0xFF && packet[1] != (superbitrf.bind_mfg_id[3] + 1 + superbitrf.packet_loss_bit)%0xFF)
+ superbitrf.packet_loss = TRUE;
+ else
+ superbitrf.packet_loss = FALSE;
+
+ // When it is a data packet, parse the packet if not busy already
+ if(!dl_msg_available && !superbitrf.packet_loss) {
+ for(i = 2; i < superbitrf.cyrf6936.rx_count; i++) {
+ parse_pprz(&superbitrf.rx_transport, packet[i]);
+
+ // When we have a full message
+ if (superbitrf.rx_transport.trans.msg_received) {
+ pprz_parse_payload(&superbitrf.rx_transport);
+ superbitrf.rx_transport.trans.msg_received = FALSE;
+ }
+ }
+ }
+ break;
+ }
+
+ if(IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) {
+ superbitrf.channels[0] = superbitrf.channel;
+ superbitrf.channels[1] = superbitrf.channel;
+
+ superbitrf.state = 1;
+ superbitrf.status = SUPERBITRF_SYNCING_B;
+ } else {
+ superbitrf.timeouts = 0;
+ superbitrf.state = 1;
+ superbitrf.status = SUPERBITRF_TRANSFER;
+ }
+ break;
+
+ /* When we receive a packet during syncing second channel B */
+ case SUPERBITRF_SYNCING_B:
+ // Check the MFG id
+ if(error && !(status & CYRF_BAD_CRC)) {
+ // Start receiving TODO: Fix nicely
+ cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2);
+ break;
+ }
+ if((IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) &&
+ (packet[0] != (~superbitrf.bind_mfg_id[2]&0xFF) || (packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF) &&
+ packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF)+1 && packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF)+2))) {
+ // Start receiving TODO: Fix nicely
+ cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2);
+ break;
+ }
+ if((IS_DSMX(superbitrf.protocol) && !SUPERBITRF_FORCE_DSM2) &&
+ (packet[0] != (superbitrf.bind_mfg_id[2]&0xFF) || (packet[1] != (superbitrf.bind_mfg_id[3]&0xFF) &&
+ packet[1] != (superbitrf.bind_mfg_id[3]&0xFF)+1 && packet[1] != (superbitrf.bind_mfg_id[3]&0xFF)+2))) {
+ // Start receiving TODO: Fix nicely
+ cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2);
+ break;
+ }
+
+ // If the CRC is wrong invert
+ if (error && (status & CYRF_BAD_CRC))
+ superbitrf.crc_seed = ~superbitrf.crc_seed;
+
+ // When we receive a data packet
+ if(packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF) && packet[1] != (superbitrf.bind_mfg_id[3]&0xFF)) {
+ superbitrf.uplink_count++;
+
+ // When it is a data packet, parse the packet if not busy already
+ if(!dl_msg_available) {
+ for(i = 2; i < superbitrf.cyrf6936.rx_count; i++) {
+ parse_pprz(&superbitrf.rx_transport, packet[i]);
+
+ // When we have a full message
+ if (superbitrf.rx_transport.trans.msg_received) {
+ pprz_parse_payload(&superbitrf.rx_transport);
+ superbitrf.rx_transport.trans.msg_received = FALSE;
+ }
+ }
+ }
+ break;
+ }
+
+ // Set the channel
+ if(superbitrf.channels[0] != superbitrf.channel) {
+ superbitrf.channels[0] = superbitrf.channel;
+ superbitrf.channel_idx = 0;
+ }
+ else {
+ superbitrf.channels[1] = superbitrf.channel;
+ superbitrf.channel_idx = 1;
+ }
+
+ // When the channels aren't the same go to transfer mode
+ if(superbitrf.channels[1] != superbitrf.channels[0]) {
+ superbitrf.state = 1;
+ superbitrf.status = SUPERBITRF_TRANSFER;
+ superbitrf.timeouts = 0;
+ }
+ break;
+
+ /* When we receive a packet during transfer */
+ case SUPERBITRF_TRANSFER:
+ // Check the MFG id
+ if(error) {
+ // Start receiving TODO: Fix nicely
+ cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2);
+ break;
+ }
+ if((IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) &&
+ (packet[0] != (~superbitrf.bind_mfg_id[2]&0xFF) || (packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF) &&
+ packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF)+1 && packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF)+2))) {
+ // Start receiving TODO: Fix nicely
+ cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2);
+ break;
+ }
+ if((IS_DSMX(superbitrf.protocol) && !SUPERBITRF_FORCE_DSM2) &&
+ (packet[0] != (superbitrf.bind_mfg_id[2]&0xFF) || (packet[1] != (superbitrf.bind_mfg_id[3]&0xFF) &&
+ packet[1] != (superbitrf.bind_mfg_id[3]&0xFF)+1 && packet[1] != (superbitrf.bind_mfg_id[3]&0xFF)+2))) {
+ // Start receiving TODO: Fix nicely
+ cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2);
+ break;
+ }
+
+ // Check if it is a RC packet
+ if(packet[1] == (~superbitrf.bind_mfg_id[3]&0xFF) || packet[1] == (superbitrf.bind_mfg_id[3]&0xFF)) {
+ superbitrf.rc_count++;
+
+ // Parse the packet
+ superbitrf_radio_to_channels(&packet[2], superbitrf.num_channels, superbitrf.resolution, superbitrf.rc_values);
+ superbitrf.rc_frame_available = TRUE;
+
+ // Calculate the timing (seperately for the channel switches)
+ if(superbitrf.crc_seed != ((superbitrf.bind_mfg_id[0] << 8) + superbitrf.bind_mfg_id[1]))
+ superbitrf.timing2 = get_sys_time_usec() - (superbitrf.timer - SUPERBITRF_RECV_TIME);
+ else
+ superbitrf.timing1 = get_sys_time_usec() - (superbitrf.timer - SUPERBITRF_RECV_SHORT_TIME);
+
+ // Go to next receive
+ superbitrf.state = 1;
+ superbitrf.timeouts = 0;
+ } else {
+ superbitrf.uplink_count++;
+
+ // Check if it is a data loss packet
+ if(packet[1] != (~superbitrf.bind_mfg_id[3] + 1 + superbitrf.packet_loss_bit) && packet[1] != (superbitrf.bind_mfg_id[3] + 1 + superbitrf.packet_loss_bit))
+ superbitrf.packet_loss = TRUE;
+ else
+ superbitrf.packet_loss = FALSE;
+
+ superbitrf.packet_loss = FALSE;
+
+ // When it is a data packet, parse the packet if not busy already
+ if(!dl_msg_available && !superbitrf.packet_loss) {
+ for(i = 2; i < superbitrf.cyrf6936.rx_count; i++) {
+ parse_pprz(&superbitrf.rx_transport, packet[i]);
+
+ // When we have a full message
+ if (superbitrf.rx_transport.trans.msg_received) {
+ pprz_parse_payload(&superbitrf.rx_transport);
+ superbitrf.rx_transport.trans.msg_received = FALSE;
+ }
+ }
+ }
+
+ // Update the state
+ superbitrf.state = 7;
+ }
+ break;
+
+ /* Should not come here */
+ default:
+ break;
+ }
+}
+
+static inline void superbitrf_send_packet_cb(bool_t error) {
+ /* Switch on the status of the superbitRF */
+ switch (superbitrf.status) {
+
+ /* When we are synchronizing */
+ case SUPERBITRF_SYNCING_A:
+ case SUPERBITRF_SYNCING_B:
+ // When we successfully or unsuccessfully send a data packet
+ if(superbitrf.state == 4)
+ superbitrf.state++;
+ break;
+
+ /* When we are in transfer mode */
+ case SUPERBITRF_TRANSFER:
+ // When we successfully or unsuccessfully send a packet
+ if(superbitrf.state == 4)
+ superbitrf.state++;
+ break;
+
+ /* Should not come here */
+ default:
+ break;
+ }
+}
+
+/**
+ * Parse a radio channel packet
+ */
+static inline void superbitrf_radio_to_channels(uint8_t* data, uint8_t nb_channels, bool_t is_11bit, int16_t* channels) {
+ int i;
+ uint8_t bit_shift = (is_11bit)? 11:10;
+ int16_t value_max = (is_11bit)? 0x07FF: 0x03FF;
+
+ for (i=0; i<7; i++) {
+ const int16_t tmp = ((data[2*i]<<8) + data[2*i+1]) & 0x7FFF;
+ const uint8_t chan = (tmp >> bit_shift) & 0x0F;
+ const int16_t val = (tmp&value_max);
+
+ if(chan < nb_channels) {
+ channels[chan] = val;
+
+ // Scale the channel
+ if(is_11bit) {
+ channels[chan] -= 0x400;
+ channels[chan] *= MAX_PPRZ/0x2AC;
+ } else {
+ channels[chan] -= 0x200;
+ channels[chan] *= MAX_PPRZ/0x156;
+ }
+ }
+ }
+}
+
+/**
+ * Generate the channels
+ */
+static inline void superbitrf_gen_dsmx_channels(void) {
+ // Calculate the DSMX channels
+ int idx = 0;
+ uint32_t id = ~((superbitrf.bind_mfg_id[0] << 24) | (superbitrf.bind_mfg_id[1] << 16) |
+ (superbitrf.bind_mfg_id[2] << 8) | (superbitrf.bind_mfg_id[3] << 0));
+ uint32_t id_tmp = id;
+
+ // While not all channels are set
+ while(idx < 23) {
+ int i;
+ int count_3_27 = 0, count_28_51 = 0, count_52_76 = 0;
+
+ id_tmp = id_tmp * 0x0019660D + 0x3C6EF35F; // Randomization
+ uint8_t next_ch = ((id_tmp >> 8) % 0x49) + 3; // Use least-significant byte and must be larger than 3
+ if (((next_ch ^ id) & 0x01 ) == 0)
+ continue;
+
+ // Go trough all already set channels
+ for (i = 0; i < idx; i++) {
+ // Channel is already used
+ if(superbitrf.channels[i] == next_ch)
+ break;
+
+ // Count the channel groups
+ if(superbitrf.channels[i] <= 27)
+ count_3_27++;
+ else if (superbitrf.channels[i] <= 51)
+ count_28_51++;
+ else
+ count_52_76++;
+ }
+
+ // When channel is already used continue
+ if (i != idx)
+ continue;
+
+ // Set the channel when channel groups aren't full
+ if ((next_ch < 28 && count_3_27 < 8) // Channels 3-27: max 8
+ || (next_ch >= 28 && next_ch < 52 && count_28_51 < 7) // Channels 28-52: max 7
+ || (next_ch >= 52 && count_52_76 < 8)) { // Channels 52-76: max 8
+ superbitrf.channels[idx++] = next_ch;
+ }
+ }
+}
+
+
+
diff --git a/sw/airborne/subsystems/datalink/superbitrf.h b/sw/airborne/subsystems/datalink/superbitrf.h
new file mode 100644
index 0000000000..e1cbb6d3bf
--- /dev/null
+++ b/sw/airborne/subsystems/datalink/superbitrf.h
@@ -0,0 +1,130 @@
+/*
+ * Copyright (C) 2013 Freek van Tienen
+ *
+ * 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 subsystems/datalink/superbitrf.h
+ * DSM2 and DSMX datalink implementation for the cyrf6936 2.4GHz radio chip trough SPI
+ */
+
+#ifndef DATALINK_SUPERBITRF_H
+#define DATALINK_SUPERBITRF_H
+
+#include "mcu_periph/gpio.h"
+#include "peripherals/cyrf6936.h"
+#include "subsystems/datalink/datalink.h"
+#include "subsystems/datalink/pprz_transport.h"
+
+/* The timings in microseconds */
+#define SUPERBITRF_BIND_RECV_TIME 10000 /**< The time to wait for a bind packet on a channel in microseconds */
+#define SUPERBITRF_SYNC_RECV_TIME 7000 /**< The time to wait for a sync packet on a channel in microseconds */
+#define SUPERBITRF_RECV_TIME 20000 /**< The time to wait for a transfer packet on a channel in microseconds */
+#define SUPERBITRF_RECV_SHORT_TIME 6000 /**< The time to wait for a transfer packet short on a channel in microseconds */
+#define SUPERBITRF_DATARECV_TIME 10000 /**< The time to wait for a data packet on a channel in microseconds */
+#define SUPERBITRF_DATARECVB_TIME 6000 /**< The time to wait for a data packet on a channel during bind in microseconds */
+
+/* The different statuses the superbitRF can be in */
+enum SuperbitRFStatus {
+ SUPERBITRF_UNINIT, /**< The chip isn't initialized */
+ SUPERBITRF_INIT_BINDING, /**< The chip is initializing binding mode */
+ SUPERBITRF_INIT_TRANSFER, /**< The chip is initializing transfer mode */
+ SUPERBITRF_BINDING, /**< The chip is in binding mode */
+ SUPERBITRF_SYNCING_A, /**< The chip is in synchronizing mode for channel A */
+ SUPERBITRF_SYNCING_B, /**< The chip is in synchronizing mode for channel B */
+ SUPERBITRF_TRANSFER, /**< The chip is in transfer mode */
+};
+
+/* The different resolutions a transmitter can be in */
+enum dsm_resolution {
+ SUPERBITRF_10_BIT_RESOLUTION = 0x00, /**< The transmitter has a 10 bit resolution */
+ SUPERBITRF_11_BIT_RESOLUTION = 0x01, /**< The transmitter has a 11 bit resolution */
+};
+
+/* The different protocols a transmitter can send */
+enum dsm_protocol {
+ DSM_DSM2_1 = 0x01, /**< The original DSM2 protocol with 1 packet of data */
+ DSM_DSM2_2 = 0x02, /**< The original DSM2 protocol with 2 packets of data */
+ DSM_DSM2P = 0x10, /**< Our own DSM2 Paparazzi protocol */
+ DSM_DSMXP = 0x11, /**< Our own DSMX Paparazzi protocol */
+ DSM_DSMX_1 = 0xA2, /**< The original DSMX protocol with 1 packet of data */
+ DSM_DSMX_2 = 0xB2, /**< The original DSMX protocol with 2 packets of data */
+};
+#define IS_DSM2(x) (x == DSM_DSM2P || x == DSM_DSM2_1 || x == DSM_DSM2_2)
+#define IS_DSMX(x) (!IS_DSM2(x))
+
+/* The superbitrf structure */
+struct SuperbitRF {
+ struct Cyrf6936 cyrf6936; /**< The cyrf chip used */
+ volatile enum SuperbitRFStatus status; /**< The status of the superbitRF */
+ uint8_t state; /**< The states each status can be in */
+ uint32_t timer; /**< The timer in microseconds */
+ bool_t timer_overflow; /**< When the timer overflows */
+ uint8_t timeouts; /**< The amount of timeouts */
+ uint32_t transfer_timeouts; /**< The amount of timeouts during transfer */
+ uint32_t resync_count; /**< The amount of resyncs needed during transfer */
+ uint8_t packet_loss_bit; /**< The packet loss indicating bit */
+ bool_t packet_loss; /**< When we have packet loss last packet */
+
+ uint8_t channels[23]; /**< The channels used for DSM2/DSMX */
+ uint8_t channel_idx; /**< The current channel index */
+ uint8_t channel; /**< The current channel number */
+ uint32_t irq_count; /**< How many interrupts are made */
+ uint32_t rx_packet_count; /**< How many packets are received(also the invalid) */
+ uint32_t tx_packet_count; /**< How many packets are send(also the invalid) */
+ uint32_t uplink_count; /**< How many valid uplink packages are received */
+ uint32_t rc_count; /**< How many valid RC packages are received */
+
+ uint8_t bind_mfg_id[4]; /**< The MFG id where the receiver is bound to */
+ uint32_t bind_mfg_id32; /**< The MFG id where the receiver is bound to in uint32 */
+ uint8_t num_channels; /**< The number of channels the transmitter has */
+ volatile enum dsm_protocol protocol; /**< The protocol the transmitter uses */
+ volatile enum dsm_resolution resolution; /**< The resolution that the transmitter has */
+ uint16_t crc_seed; /**< The CRC seed that is calculated with the bind MFG id */
+ uint8_t sop_col; /**< The sop code column number calculated with the bind MFG id */
+ uint8_t data_col; /**< The data code column number calculated with the bind MFG id */
+
+ bool_t rc_frame_available; /**< When a RC frame is available */
+ uint32_t timing1; /**< Time between last receive in microseconds */
+ uint32_t timing2; /**< Time between second last receive in microseconds */
+ int16_t rc_values[14]; /**< The rc values from the packet */
+
+ struct pprz_transport rx_transport; /**< The receive transport */
+
+ uint8_t tx_buffer[128]; /**< The transmit buffer */
+ uint8_t tx_insert_idx; /**< The transmit buffer insert index */
+ uint8_t tx_extract_idx; /**< The transmit buffer extract index */
+};
+
+/* The superbitrf functions and structures */
+extern struct SuperbitRF superbitrf;
+void superbitrf_init(void);
+void superbitrf_event(void);
+
+/* The datalink defines */
+#define SuperbitRFInit() { }//superbitrf_init(); }
+#define SuperbitRFCheckFreeSpace(_x) (((superbitrf.tx_insert_idx+1) %128) != superbitrf.tx_extract_idx)
+#define SuperbitRFTransmit(_x) { \
+ superbitrf.tx_buffer[superbitrf.tx_insert_idx] = _x; \
+ superbitrf.tx_insert_idx = (superbitrf.tx_insert_idx+1) %128; \
+ }
+#define SuperbitRFSendMessage() { }
+#define SuperbitRFCheckAndParse() { }
+
+#endif /* DATALINK_SUPERBITRF_H */
diff --git a/sw/airborne/subsystems/datalink/udp.c b/sw/airborne/subsystems/datalink/udp.c
index 9d9a717490..88ab870c1d 100644
--- a/sw/airborne/subsystems/datalink/udp.c
+++ b/sw/airborne/subsystems/datalink/udp.c
@@ -26,7 +26,7 @@
//Check if variables are set and else define them
#ifndef LINK_HOST
-#define LINK_HOST "192.168.1.0"
+#define LINK_HOST "192.168.1.255"
#endif
#ifndef LINK_PORT
#define LINK_PORT 4242
diff --git a/sw/airborne/subsystems/electrical.c b/sw/airborne/subsystems/electrical.c
index d0075cdd0c..e7103e8075 100644
--- a/sw/airborne/subsystems/electrical.c
+++ b/sw/airborne/subsystems/electrical.c
@@ -1,3 +1,30 @@
+/*
+ * Copyright (C) 2010-2013 The Paparazzi Team
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file subsystems/electrical.c
+ *
+ * Implemnetation for electrical status: supply voltage, current, battery status, etc.
+ */
+
#include "subsystems/electrical.h"
#include "mcu_periph/adc.h"
@@ -19,6 +46,15 @@
#define COMMAND_CURRENT_ESTIMATION COMMAND_THRUST
#endif
+#ifndef BAT_CHECKER_DELAY
+#define BAT_CHECKER_DELAY 5
+#endif
+
+#define ELECTRICAL_PERIODIC_FREQ 10
+
+PRINT_CONFIG_VAR(LOW_BAT_LEVEL)
+PRINT_CONFIG_VAR(CRITIC_BAT_LEVEL)
+
struct Electrical electrical;
#if defined ADC_CHANNEL_VSUPPLY || defined ADC_CHANNEL_CURRENT || defined MILLIAMP_AT_FULL_THROTTLE
@@ -50,6 +86,9 @@ void electrical_init(void) {
electrical.vsupply = 0;
electrical.current = 0;
+ electrical.bat_low = FALSE;
+ electrical.bat_critical = FALSE;
+
#if defined ADC_CHANNEL_VSUPPLY
adc_buf_channel(ADC_CHANNEL_VSUPPLY, &electrical_priv.vsupply_adc_buf, DEFAULT_AV_NB_SAMPLE);
#endif
@@ -64,6 +103,9 @@ PRINT_CONFIG_VAR(CURRENT_ESTIMATION_NONLINEARITY)
}
void electrical_periodic(void) {
+ static uint8_t bat_low_counter = 0;
+ static uint8_t bat_critical_counter = 0;
+
#if defined(ADC_CHANNEL_VSUPPLY) && !defined(SITL)
electrical.vsupply = 10 * VoltageOfAdc((electrical_priv.vsupply_adc_buf.sum/electrical_priv.vsupply_adc_buf.av_nb_sample));
#endif
@@ -93,4 +135,29 @@ void electrical_periodic(void) {
electrical.current = b - pow((pow(b,electrical_priv.nonlin_factor)-pow((b*x),electrical_priv.nonlin_factor)), (1./electrical_priv.nonlin_factor));
#endif /* ADC_CHANNEL_CURRENT */
+
+ if (electrical.vsupply < LOW_BAT_LEVEL * 10) {
+ if (bat_low_counter > 0)
+ bat_low_counter--;
+ if (bat_low_counter == 0)
+ electrical.bat_low = TRUE;
+ }
+ else {
+ // reset battery low status and counter
+ bat_low_counter = BAT_CHECKER_DELAY * ELECTRICAL_PERIODIC_FREQ;
+ electrical.bat_low = FALSE;
+ }
+
+ if (electrical.vsupply < CRITIC_BAT_LEVEL * 10) {
+ if (bat_critical_counter > 0)
+ bat_critical_counter--;
+ if (bat_critical_counter == 0)
+ electrical.bat_critical = TRUE;
+ }
+ else {
+ // reset battery critical status and counter
+ bat_critical_counter = BAT_CHECKER_DELAY * ELECTRICAL_PERIODIC_FREQ;
+ electrical.bat_critical = FALSE;
+ }
+
}
diff --git a/sw/airborne/subsystems/electrical.h b/sw/airborne/subsystems/electrical.h
index d9b70f9a2f..81d70fbda2 100644
--- a/sw/airborne/subsystems/electrical.h
+++ b/sw/airborne/subsystems/electrical.h
@@ -1,13 +1,55 @@
+/*
+ * Copyright (C) 2010-2013 The Paparazzi Team
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file subsystems/electrical.h
+ *
+ * Interface for electrical status: supply voltage, current, battery status, etc.
+ */
+
#ifndef SUBSYSTEMS_ELECTRICAL_H
#define SUBSYSTEMS_ELECTRICAL_H
#include "std.h"
+#include "generated/airframe.h"
+
+/** low battery level in Volts (for 3S LiPo) */
+#ifndef LOW_BAT_LEVEL
+#define LOW_BAT_LEVEL 10.5
+#endif
+
+
+/** critical battery level in Volts (for 3S LiPo) */
+#ifndef CRITIC_BAT_LEVEL
+#define CRITIC_BAT_LEVEL 9.8
+#endif
+
struct Electrical {
- uint16_t vsupply; ///< supply voltage in decivolts
- int32_t current; ///< current in milliamps
- int32_t consumed; ///< consumption in mAh
+ uint16_t vsupply; ///< supply voltage in decivolts
+ int32_t current; ///< current in milliamps
+ int32_t consumed; ///< consumption in mAh
+ bool_t bat_low; ///< battery low status
+ bool_t bat_critical; ///< battery critical status
};
diff --git a/sw/airborne/subsystems/gps/gps_ardrone2.c b/sw/airborne/subsystems/gps/gps_ardrone2.c
index 0326f0dd64..2feba4c765 100644
--- a/sw/airborne/subsystems/gps/gps_ardrone2.c
+++ b/sw/airborne/subsystems/gps/gps_ardrone2.c
@@ -26,6 +26,10 @@
* ARdrone 2 gps trough navdata for the SDK version and only works in combination with the ahrs ardrone2.
*/
+#ifdef ARDRONE2_DEBUG
+# include
+#endif
+
#include "subsystems/gps.h"
#include "math/pprz_geodetic_double.h"
@@ -37,6 +41,10 @@ void gps_impl_init( void ) {
void gps_ardrone2_parse(navdata_gps_t *navdata_gps) {
int i;
+
+#ifdef ARDRONE2_DEBUG
+ printf("state = %d\n", navdata_gps->gps_state);
+#endif
// Set the lla double struct from the navdata
struct LlaCoor_d gps_lla_d;
gps_lla_d.lat = RadOfDeg(navdata_gps->lat);
diff --git a/sw/airborne/subsystems/gps/gps_sim_nps.c b/sw/airborne/subsystems/gps/gps_sim_nps.c
index 55c935af60..546d32d549 100644
--- a/sw/airborne/subsystems/gps/gps_sim_nps.c
+++ b/sw/airborne/subsystems/gps/gps_sim_nps.c
@@ -22,7 +22,14 @@
#include "subsystems/gps/gps_sim_nps.h"
#include "subsystems/gps.h"
+#if GPS_USE_LATLONG
+/* currently needed to get nav_utm_zone0 */
+#include "subsystems/navigation/common_nav.h"
+#include "math/pprz_geodetic_float.h"
+#endif
+
bool_t gps_available;
+bool_t gps_has_fix;
void gps_feed_value() {
gps.ecef_pos.x = sensors.gps.ecef_pos.x * 100.;
@@ -36,10 +43,47 @@ void gps_feed_value() {
gps.lla_pos.lon = sensors.gps.lla_pos.lon * 1e7;
gps.lla_pos.alt = sensors.gps.lla_pos.alt * 1000.;
gps.hmsl = sensors.gps.hmsl * 1000.;
- gps.fix = GPS_FIX_3D;
+
+ /* calc NED speed from ECEF */
+ struct LtpDef_d ref_ltp;
+ ltp_def_from_ecef_d(&ref_ltp, &sensors.gps.ecef_pos);
+ struct NedCoor_d ned_vel_d;
+ ned_of_ecef_vect_d(&ned_vel_d, &ref_ltp, &sensors.gps.ecef_vel);
+ gps.ned_vel.x = ned_vel_d.x * 100;
+ gps.ned_vel.y = ned_vel_d.y * 100;
+ gps.ned_vel.z = ned_vel_d.z * 100;
+
+ /* horizontal and 3d ground speed in cm/s */
+ gps.gspeed = sqrt(ned_vel_d.x * ned_vel_d.x + ned_vel_d.y * ned_vel_d.y) * 100;
+ gps.speed_3d = sqrt(ned_vel_d.x * ned_vel_d.x + ned_vel_d.y * ned_vel_d.y + ned_vel_d.z * ned_vel_d.z) * 100;
+
+ /* ground course in radians * 1e7 */
+ gps.course = atan2(ned_vel_d.y, ned_vel_d.x) * 1e7;
+
+#if GPS_USE_LATLONG
+ /* Computes from (lat, long) in the referenced UTM zone */
+ struct LlaCoor_f lla_f;
+ lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
+ lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
+ struct UtmCoor_f utm_f;
+ utm_f.zone = nav_utm_zone0;
+ /* convert to utm */
+ utm_of_lla_f(&utm_f, &lla_f);
+ /* copy results of utm conversion */
+ gps.utm_pos.east = utm_f.east*100;
+ gps.utm_pos.north = utm_f.north*100;
+ gps.utm_pos.alt = gps.lla_pos.alt;
+ gps.utm_pos.zone = nav_utm_zone0;
+#endif
+
+ if (gps_has_fix)
+ gps.fix = GPS_FIX_3D;
+ else
+ gps.fix = GPS_FIX_NONE;
gps_available = TRUE;
}
void gps_impl_init() {
gps_available = FALSE;
+ gps_has_fix = TRUE;
}
diff --git a/sw/airborne/subsystems/gps/gps_sim_nps.h b/sw/airborne/subsystems/gps/gps_sim_nps.h
index e1dd5fc05d..9c327590a2 100644
--- a/sw/airborne/subsystems/gps/gps_sim_nps.h
+++ b/sw/airborne/subsystems/gps/gps_sim_nps.h
@@ -6,6 +6,7 @@
#define GPS_NB_CHANNELS 16
extern bool_t gps_available;
+extern bool_t gps_has_fix;
extern void gps_feed_value();
diff --git a/sw/airborne/subsystems/imu.c b/sw/airborne/subsystems/imu.c
index daf7e478b2..0db6ba77aa 100644
--- a/sw/airborne/subsystems/imu.c
+++ b/sw/airborne/subsystems/imu.c
@@ -29,6 +29,20 @@
#if DOWNLINK
#include "subsystems/datalink/telemetry.h"
+#if USE_IMU_FLOAT
+
+static void send_accel(void) {
+ DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, DefaultDevice,
+ &imuf.accel.x, &imuf.accel.y, &imuf.accel.z);
+}
+
+static void send_gyro(void) {
+ DOWNLINK_SEND_IMU_GYRO(DefaultChannel, DefaultDevice,
+ &imuf.gyro.p, &imuf.gyro.q, &imuf.gyro.r);
+}
+
+#else // !USE_IMU_FLOAT
+
static void send_accel_raw(void) {
DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice,
&imu.accel_unscaled.x, &imu.accel_unscaled.y, &imu.accel_unscaled.z);
@@ -90,9 +104,12 @@ static void send_mag_calib(void) {
}
#endif
+#endif // !USE_IMU_FLOAT
+
#endif
struct Imu imu;
+struct ImuFloat imuf;
void imu_init(void) {
@@ -123,6 +140,10 @@ INFO("Magnetometer neutrals are set to zero, you should calibrate!")
INT32_RMAT_OF_EULERS(imu.body_to_imu_rmat, body_to_imu_eulers);
#if DOWNLINK
+ register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL", send_accel);
+ register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO", send_gyro);
+#if USE_IMU_FLOAT
+#else // !USE_IMU_FLOAT
register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL_RAW", send_accel_raw);
register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL_SCALED", send_accel_scaled);
register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL", send_accel);
@@ -134,23 +155,22 @@ INFO("Magnetometer neutrals are set to zero, you should calibrate!")
register_periodic_telemetry(DefaultPeriodic, "IMU_MAG_SCALED", send_mag_scaled);
register_periodic_telemetry(DefaultPeriodic, "IMU_MAG", send_mag);
register_periodic_telemetry(DefaultPeriodic, "IMU_MAG_CURRENT_CALIBRATION", send_mag_calib);
-#endif
-#endif
+#endif // USE_MAGNETOMETER
+#endif // !USE_IMU_FLOAT
+#endif // DOWNLINK
imu_impl_init();
}
-void imu_float_init(struct ImuFloat* imuf) {
-
+void imu_float_init(void) {
/*
Compute quaternion and rotation matrix
for conversions between body and imu frame
*/
- EULERS_ASSIGN(imuf->body_to_imu_eulers,
+ 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_NORMALIZE(imuf->body_to_imu_quat);
- FLOAT_RMAT_OF_EULERS(imuf->body_to_imu_rmat, imuf->body_to_imu_eulers);
-
+ FLOAT_QUAT_OF_EULERS(imuf.body_to_imu_quat, imuf.body_to_imu_eulers);
+ FLOAT_QUAT_NORMALIZE(imuf.body_to_imu_quat);
+ FLOAT_RMAT_OF_EULERS(imuf.body_to_imu_rmat, imuf.body_to_imu_eulers);
}
diff --git a/sw/airborne/subsystems/imu.h b/sw/airborne/subsystems/imu.h
index 72f759f7ef..1d082a9f58 100644
--- a/sw/airborne/subsystems/imu.h
+++ b/sw/airborne/subsystems/imu.h
@@ -64,10 +64,11 @@ struct ImuFloat {
uint32_t sample_count;
};
-extern void imu_float_init(struct ImuFloat* imuf);
+
/** global IMU state */
extern struct Imu imu;
+extern struct ImuFloat imuf;
/* underlying hardware */
#ifdef IMU_TYPE_H
@@ -75,7 +76,7 @@ extern struct Imu imu;
#endif
extern void imu_init(void);
-
+extern void imu_float_init(void);
#if !defined IMU_BODY_TO_IMU_PHI && !defined IMU_BODY_TO_IMU_THETA && !defined IMU_BODY_TO_IMU_PSI
#define IMU_BODY_TO_IMU_PHI 0
diff --git a/sw/airborne/subsystems/imu/imu_ardrone2_raw.c b/sw/airborne/subsystems/imu/imu_ardrone2_raw.c
index b43a6ab357..a16afa4938 100644
--- a/sw/airborne/subsystems/imu/imu_ardrone2_raw.c
+++ b/sw/airborne/subsystems/imu/imu_ardrone2_raw.c
@@ -30,23 +30,13 @@
#include "mcu_periph/uart.h"
void imu_impl_init(void) {
- imu_data_available = FALSE;
navdata_init();
}
void imu_periodic(void) {
- navdata_update();
- //checks if the navboard has a new dataset ready
- if (navdata_imu_available == TRUE) {
- navdata_imu_available = FALSE;
- RATES_ASSIGN(imu.gyro_unscaled, navdata->vx, navdata->vy, navdata->vz);
- VECT3_ASSIGN(imu.accel_unscaled, navdata->ax, navdata->ay, navdata->az);
- VECT3_ASSIGN(imu.mag_unscaled, navdata->mx, navdata->my, navdata->mz);
- imu_data_available = TRUE;
- }
- else {
- imu_data_available = FALSE;
- }
+}
+
+void navdata_event(void) {
#ifdef USE_UART1
uart1_handler();
diff --git a/sw/airborne/subsystems/imu/imu_ardrone2_raw.h b/sw/airborne/subsystems/imu/imu_ardrone2_raw.h
index ce15a2c2d5..cbc7c33b19 100644
--- a/sw/airborne/subsystems/imu/imu_ardrone2_raw.h
+++ b/sw/airborne/subsystems/imu/imu_ardrone2_raw.h
@@ -31,16 +31,100 @@
#include "generated/airframe.h"
#include "navdata.h"
-int imu_data_available;
+#if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN
+#define IMU_MAG_X_SIGN 1
+#define IMU_MAG_Y_SIGN 1
+#define IMU_MAG_Z_SIGN 1
+#endif
+#if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN
+#define IMU_GYRO_P_SIGN 1
+#define IMU_GYRO_Q_SIGN 1
+#define IMU_GYRO_R_SIGN 1
+#endif
+#if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN
+#define IMU_ACCEL_X_SIGN 1
+#define IMU_ACCEL_Y_SIGN 1
+#define IMU_ACCEL_Z_SIGN 1
+#endif
+
+/** default gyro sensitivy and neutral from the datasheet
+ * MPU with 2000 deg/s
+ */
+#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
+#define IMU_GYRO_P_SENS 4.359
+#define IMU_GYRO_P_SENS_NUM 4359
+#define IMU_GYRO_P_SENS_DEN 1000
+#define IMU_GYRO_Q_SENS 4.359
+#define IMU_GYRO_Q_SENS_NUM 4359
+#define IMU_GYRO_Q_SENS_DEN 1000
+#define IMU_GYRO_R_SENS 4.359
+#define IMU_GYRO_R_SENS_NUM 4359
+#define IMU_GYRO_R_SENS_DEN 1000
+#endif
+#if !defined IMU_GYRO_P_NEUTRAL & !defined IMU_GYRO_Q_NEUTRAL & !defined IMU_GYRO_R_NEUTRAL
+#define IMU_GYRO_P_NEUTRAL 0
+#define IMU_GYRO_Q_NEUTRAL 0
+#define IMU_GYRO_R_NEUTRAL 0
+#endif
+
+/** default accel sensitivy from the datasheet
+ * 512 LSB/g
+ */
+#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
+#define IMU_ACCEL_X_SENS 19.5
+#define IMU_ACCEL_X_SENS_NUM 195
+#define IMU_ACCEL_X_SENS_DEN 10
+#define IMU_ACCEL_Y_SENS 19.5
+#define IMU_ACCEL_Y_SENS_NUM 195
+#define IMU_ACCEL_Y_SENS_DEN 10
+#define IMU_ACCEL_Z_SENS 19.5
+#define IMU_ACCEL_Z_SENS_NUM 195
+#define IMU_ACCEL_Z_SENS_DEN 10
+#endif
+
+#if !defined IMU_ACCEL_X_NEUTRAL & !defined IMU_ACCEL_Y_NEUTRAL & !defined IMU_ACCEL_Z_NEUTRAL
+#define IMU_ACCEL_X_NEUTRAL 2048
+#define IMU_ACCEL_Y_NEUTRAL 2048
+#define IMU_ACCEL_Z_NEUTRAL 2048
+#endif
+
+#if !defined IMU_MAG_X_SENS & !defined IMU_MAG_Y_SENS & !defined IMU_MAG_Z_SENS
+#define IMU_MAG_X_SENS 16.0
+#define IMU_MAG_X_SENS_NUM 16
+#define IMU_MAG_X_SENS_DEN 1
+#define IMU_MAG_Y_SENS 16.0
+#define IMU_MAG_Y_SENS_NUM 16
+#define IMU_MAG_Y_SENS_DEN 1
+#define IMU_MAG_Z_SENS 16.0
+#define IMU_MAG_Z_SENS_NUM 16
+#define IMU_MAG_Z_SENS_DEN 1
+#endif
+
+#if !defined IMU_MAG_X_NEUTRAL & !defined IMU_MAG_Y_NEUTRAL & !defined IMU_MAG_Z_NEUTRAL
+#define IMU_MAG_X_NEUTRAL 0
+#define IMU_MAG_Y_NEUTRAL 0
+#define IMU_MAG_Z_NEUTRAL 0
+#endif
+
+
+
+void navdata_event(void);
static inline void imu_ardrone2_event ( void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void))
{
- if (imu_data_available) {
- imu_data_available = FALSE;
+ navdata_update();
+ //checks if the navboard has a new dataset ready
+ if (navdata_imu_available == TRUE) {
+ navdata_imu_available = FALSE;
+ RATES_ASSIGN(imu.gyro_unscaled, navdata.vx, -navdata.vy, -navdata.vz);
+ VECT3_ASSIGN(imu.accel_unscaled, navdata.ax, 4096-navdata.ay, 4096-navdata.az);
+ VECT3_ASSIGN(imu.mag_unscaled, -navdata.mx, -navdata.my, -navdata.mz);
+
_gyro_handler();
_accel_handler();
_mag_handler();
}
+ navdata_event();
}
#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \
diff --git a/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c
index bd463158df..ef708b53ee 100644
--- a/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c
+++ b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c
@@ -40,11 +40,25 @@ PRINT_CONFIG_VAR(ASPIRIN_2_SPI_SLAVE_IDX)
#endif
PRINT_CONFIG_VAR(ASPIRIN_2_SPI_DEV)
-/* gyro internal lowpass frequency */
+/* MPU60x0 gyro/accel internal lowpass frequency */
#if !defined ASPIRIN_2_LOWPASS_FILTER && !defined ASPIRIN_2_SMPLRT_DIV
+#if (PERIODIC_FREQUENCY == 60) || (PERIODIC_FREQUENCY == 120)
+/* Accelerometer: Bandwidth 44Hz, Delay 4.9ms
+ * Gyroscope: Bandwidth 42Hz, Delay 4.8ms sampling 1kHz
+ */
+#define ASPIRIN_2_LOWPASS_FILTER MPU60X0_DLPF_42HZ
+#define ASPIRIN_2_SMPLRT_DIV 9
+PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz at 1kHz internal sampling")
+#elif PERIODIC_FREQUENCY == 512
+/* Accelerometer: Bandwidth 260Hz, Delay 0ms
+ * Gyroscope: Bandwidth 256Hz, Delay 0.98ms sampling 8kHz
+ */
#define ASPIRIN_2_LOWPASS_FILTER MPU60X0_DLPF_256HZ
-#define ASPIRIN_2_SMPLRT_DIV 1
-//PRINT_CONFIG_MSG("Gyro/Accel output rate is 500Hz")
+#define ASPIRIN_2_SMPLRT_DIV 3
+PRINT_CONFIG_MSG("Gyro/Accel output rate is 2kHz at 8kHz internal sampling")
+#else
+#error Non-default PERIODIC_FREQUENCY: please define ASPIRIN_2_LOWPASS_FILTER and ASPIRIN_2_SMPLRT_DIV.
+#endif
#endif
PRINT_CONFIG_VAR(ASPIRIN_2_LOWPASS_FILTER)
PRINT_CONFIG_VAR(ASPIRIN_2_SMPLRT_DIV)
@@ -160,10 +174,28 @@ void imu_aspirin2_event(void)
-imu_aspirin2.mpu.data_accel.vect.x,
imu_aspirin2.mpu.data_accel.vect.z);
VECT3_ASSIGN(imu.mag_unscaled, -mag.x, -mag.y, mag.z);
+#else
+#ifdef LISA_S
+#ifdef LISA_S_UPSIDE_DOWN
+ RATES_ASSIGN(imu.gyro_unscaled,
+ imu_aspirin2.mpu.data_rates.rates.p,
+ -imu_aspirin2.mpu.data_rates.rates.q,
+ -imu_aspirin2.mpu.data_rates.rates.r);
+ VECT3_ASSIGN(imu.accel_unscaled,
+ imu_aspirin2.mpu.data_accel.vect.x,
+ -imu_aspirin2.mpu.data_accel.vect.y,
+ -imu_aspirin2.mpu.data_accel.vect.z);
+ VECT3_ASSIGN(imu.mag_unscaled, mag.x, -mag.y, -mag.z);
+#else
+ RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates);
+ VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect);
+ VECT3_COPY(imu.mag_unscaled, mag);
+#endif
#else
RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates);
VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect);
VECT3_ASSIGN(imu.mag_unscaled, mag.y, -mag.x, mag.z)
+#endif
#endif
imu_aspirin2.mpu.data_available = FALSE;
imu_aspirin2.gyro_valid = TRUE;
diff --git a/sw/airborne/subsystems/imu/imu_aspirin_i2c.c b/sw/airborne/subsystems/imu/imu_aspirin_i2c.c
index 31b09932f9..a94ce32a77 100644
--- a/sw/airborne/subsystems/imu/imu_aspirin_i2c.c
+++ b/sw/airborne/subsystems/imu/imu_aspirin_i2c.c
@@ -96,7 +96,7 @@ void imu_impl_init(void)
// With CS tied high to VDD I/O, the ADXL345 is in I2C mode
#ifdef ASPIRIN_I2C_CS_PORT
gpio_setup_output(ASPIRIN_I2C_CS_PORT, ASPIRIN_I2C_CS_PIN);
- gpio_output_high(ASPIRIN_I2C_CS_PORT, ASPIRIN_I2C_CS_PIN);
+ gpio_set(ASPIRIN_I2C_CS_PORT, ASPIRIN_I2C_CS_PIN);
#endif
/* Gyro configuration and initalization */
diff --git a/sw/airborne/subsystems/imu/imu_gl1.c b/sw/airborne/subsystems/imu/imu_gl1.c
new file mode 100644
index 0000000000..eb035d6704
--- /dev/null
+++ b/sw/airborne/subsystems/imu/imu_gl1.c
@@ -0,0 +1,154 @@
+/*
+ * Copyright (C) 2010 Antoine Drouin
+ * 2013 Felix Ruess
+ * 2013 Eduardo Lavratti
+ *
+ * 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 subsystems/imu/imu_gl1.c
+ * Driver for I2C IMU using L3G4200, ADXL345, HMC5883 and BMP085.
+ */
+
+#include "subsystems/imu.h"
+#include "mcu_periph/i2c.h"
+
+PRINT_CONFIG_VAR(GL1_I2C_DEV)
+
+/** adxl345 accelerometer output rate, lowpass is set to half of rate */
+#ifndef GL1_ACCEL_RATE
+# if PERIODIC_FREQUENCY <= 60
+# define GL1_ACCEL_RATE ADXL345_RATE_50HZ
+# elif PERIODIC_FREQUENCY <= 120
+# define GL1_ACCEL_RATE ADXL345_RATE_100HZ
+# else
+# define GL1_ACCEL_RATE ADXL345_RATE_200HZ
+# endif
+#endif
+PRINT_CONFIG_VAR(GL1_ACCEL_RATE)
+
+/** gyro internal lowpass frequency */
+#ifndef GL1_GYRO_LOWPASS
+# if PERIODIC_FREQUENCY <= 60
+# define GL1_GYRO_LOWPASS L3G4200_DLPF_1
+# elif PERIODIC_FREQUENCY <= 120
+# define GL1_GYRO_LOWPASS L3G4200_DLPF_2
+# else
+# define GL1_GYRO_LOWPASS L3G4200_DLPF_3
+# endif
+#endif
+PRINT_CONFIG_VAR(GL1_GYRO_LOWPASS)
+
+/** gyro sample rate divider */
+#ifndef GL1_GYRO_SMPLRT
+# if PERIODIC_FREQUENCY <= 60
+# define GL1_GYRO_SMPLRT L3G4200_DR_100Hz
+PRINT_CONFIG_MSG("Gyro output rate is 100Hz")
+# else
+# define GL1_GYRO_SMPLRT L3G4200_DR_100Hz
+PRINT_CONFIG_MSG("Gyro output rate is 100Hz")
+# endif
+#endif
+PRINT_CONFIG_VAR(GL1_GYRO_SMPLRT)
+
+#ifdef GL1_GYRO_SCALE
+# define L3G4200_SCALE GL1_GYRO_SCALE
+# else
+# define L3G4200_SCALE L3G4200_SCALE_2000
+#endif
+PRINT_CONFIG_VAR(L3G4200_SCALE)
+
+struct ImuGL1I2c imu_gl1;
+
+void imu_impl_init(void)
+{
+ imu_gl1.accel_valid = FALSE;
+ imu_gl1.gyro_valid = FALSE;
+ imu_gl1.mag_valid = FALSE;
+
+ /* Set accel configuration */
+ adxl345_i2c_init(&imu_gl1.acc_adxl, &(GL1_I2C_DEV), ADXL345_ADDR);
+ // set the data rate
+ imu_gl1.acc_adxl.config.rate = GL1_ACCEL_RATE;
+ /// @todo drdy int handling for adxl345
+ //imu_aspirin.acc_adxl.config.drdy_int_enable = TRUE;
+
+
+ /* Gyro configuration and initalization */
+ l3g4200_init(&imu_gl1.gyro_l3g, &(GL1_I2C_DEV), L3G4200_ADDR_ALT);
+ /* change the default config */
+ // output data rate, bandwidth, enable axis (0x1f = 100 ODR, 25hz) (0x5f = 200hz ODR, 25hz)
+ imu_gl1.gyro_l3g.config.ctrl_reg1 = ((GL1_GYRO_SMPLRT<<6) | (GL1_GYRO_LOWPASS<<4) | 0xf);
+ // senstivity
+ imu_gl1.gyro_l3g.config.ctrl_reg4 = (L3G4200_SCALE<<4) | 0x00;
+ // filter config
+ imu_gl1.gyro_l3g.config.ctrl_reg5 = 0x00; // only first LPF active
+
+
+ /* initialize mag and set default options */
+ hmc58xx_init(&imu_gl1.mag_hmc, &(GL1_I2C_DEV), HMC58XX_ADDR);
+ imu_gl1.mag_hmc.type = HMC_TYPE_5883;
+}
+
+
+void imu_periodic(void)
+{
+ adxl345_i2c_periodic(&imu_gl1.acc_adxl);
+
+ // Start reading the latest gyroscope data
+ l3g4200_periodic(&imu_gl1.gyro_l3g);
+
+ // Read HMC58XX at 50Hz (main loop for rotorcraft: 512Hz)
+ RunOnceEvery(10, hmc58xx_periodic(&imu_gl1.mag_hmc));
+}
+
+void imu_gl1_i2c_event(void)
+{
+ adxl345_i2c_event(&imu_gl1.acc_adxl);
+ if (imu_gl1.acc_adxl.data_available) {
+ VECT3_COPY(imu.accel_unscaled, imu_gl1.acc_adxl.data.vect);
+ imu.accel_unscaled.x = imu_gl1.acc_adxl.data.vect.y;
+ imu.accel_unscaled.y = imu_gl1.acc_adxl.data.vect.x;
+ imu.accel_unscaled.z = -imu_gl1.acc_adxl.data.vect.z;
+ imu_gl1.acc_adxl.data_available = FALSE;
+ imu_gl1.accel_valid = TRUE;
+ }
+
+ /* If the lg34200 I2C transaction has succeeded: convert the data */
+ l3g4200_event(&imu_gl1.gyro_l3g);
+ if (imu_gl1.gyro_l3g.data_available) {
+ RATES_COPY(imu.gyro_unscaled, imu_gl1.gyro_l3g.data.rates);
+ imu.gyro_unscaled.p = imu_gl1.gyro_l3g.data.rates.q;
+ imu.gyro_unscaled.q = imu_gl1.gyro_l3g.data.rates.p;
+ imu.gyro_unscaled.r = -imu_gl1.gyro_l3g.data.rates.r;
+ imu_gl1.gyro_l3g.data_available = FALSE;
+ imu_gl1.gyro_valid = TRUE;
+ }
+
+ /* HMC58XX event task */
+ hmc58xx_event(&imu_gl1.mag_hmc);
+ if (imu_gl1.mag_hmc.data_available) {
+ // VECT3_COPY(imu.mag_unscaled, imu_gl1.mag_hmc.data.vect);
+ imu.mag_unscaled.y = imu_gl1.mag_hmc.data.vect.x;
+ imu.mag_unscaled.x = imu_gl1.mag_hmc.data.vect.y;
+ imu.mag_unscaled.z = -imu_gl1.mag_hmc.data.vect.z;
+ imu_gl1.mag_hmc.data_available = FALSE;
+ imu_gl1.mag_valid = TRUE;
+ }
+}
diff --git a/sw/airborne/subsystems/imu/imu_gl1.h b/sw/airborne/subsystems/imu/imu_gl1.h
new file mode 100644
index 0000000000..a222ca6459
--- /dev/null
+++ b/sw/airborne/subsystems/imu/imu_gl1.h
@@ -0,0 +1,72 @@
+/*
+ * Copyright (C) 2010 Antoine Drouin
+ * 2013 Felix Ruess
+ * 2013 Eduardo Lavratti
+ *
+ * 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 subsystems/imu/imu_gl1.h
+ * Interface for I2c IMU using using L3G4200, ADXL345, HMC5883 and BMP085.
+ */
+
+
+#ifndef IMU_GL1_H
+#define IMU_GL1_H
+
+#include "generated/airframe.h"
+#include "subsystems/imu.h"
+
+/* include default GL1 sensitivity/channel definitions */
+#include "subsystems/imu/imu_gl1_defaults.h"
+
+#include "peripherals/l3g4200.h"
+#include "peripherals/hmc58xx.h"
+#include "peripherals/adxl345_i2c.h"
+
+struct ImuGL1I2c {
+ volatile uint8_t accel_valid;
+ volatile uint8_t gyro_valid;
+ volatile uint8_t mag_valid;
+ struct Adxl345_I2c acc_adxl;
+ struct L3g4200 gyro_l3g;
+ struct Hmc58xx mag_hmc;
+};
+
+extern struct ImuGL1I2c imu_gl1;
+
+extern void imu_gl1_i2c_event(void);
+
+static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) {
+ imu_gl1_i2c_event();
+ if (imu_gl1.gyro_valid) {
+ imu_gl1.gyro_valid = FALSE;
+ _gyro_handler();
+ }
+ if (imu_gl1.accel_valid) {
+ imu_gl1.accel_valid = FALSE;
+ _accel_handler();
+ }
+ if (imu_gl1.mag_valid) {
+ imu_gl1.mag_valid = FALSE;
+ _mag_handler();
+ }
+}
+
+#endif /* IMU_GL1_H */
diff --git a/sw/airborne/subsystems/imu/imu_gl1_defaults.h b/sw/airborne/subsystems/imu/imu_gl1_defaults.h
new file mode 100644
index 0000000000..874afd3cd3
--- /dev/null
+++ b/sw/airborne/subsystems/imu/imu_gl1_defaults.h
@@ -0,0 +1,94 @@
+/*
+ * Copyright (C) 2010-2013 The Paparazzi Team
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file subsystems/imu/imu_gl1_defaults.h
+ * Default sensitivity definitions for IMU GL1.
+ */
+
+
+#ifndef IMU_GL1_DEFAULTS_H
+#define IMU_GL1_DEFAULTS_H
+
+#include "generated/airframe.h"
+
+#if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN
+#define IMU_MAG_X_SIGN 1
+#define IMU_MAG_Y_SIGN 1
+#define IMU_MAG_Z_SIGN 1
+#endif
+#if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN
+#define IMU_GYRO_P_SIGN 1
+#define IMU_GYRO_Q_SIGN 1
+#define IMU_GYRO_R_SIGN 1
+#endif
+#if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN
+#define IMU_ACCEL_X_SIGN 1
+#define IMU_ACCEL_Y_SIGN 1
+#define IMU_ACCEL_Z_SIGN 1
+#endif
+
+
+/** default gyro sensitivy and neutral from the datasheet
+ * L3G4200 has 8.75 LSB/(deg/s)
+ * sens = 1/xxx * pi/180 * 2^INT32_RATE_FRAC
+ * sens = 1/xxx * pi/180 * 4096 = ?????
+ *
+ * 250deg = 114.28 = 0.625
+ * 500deg = 57.14 = 1.25
+ * 2000deg = 14.28 = 5.006
+ */
+#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
+#define IMU_GYRO_P_SENS 5.006
+#define IMU_GYRO_P_SENS_NUM 2503
+#define IMU_GYRO_P_SENS_DEN 500
+#define IMU_GYRO_Q_SENS 5.006
+#define IMU_GYRO_Q_SENS_NUM 2503
+#define IMU_GYRO_Q_SENS_DEN 500
+#define IMU_GYRO_R_SENS 5.006
+#define IMU_GYRO_R_SENS_NUM 2503
+#define IMU_GYRO_R_SENS_DEN 500
+#endif
+
+/** default accel sensitivy from the ADXL345 datasheet
+ * sensitivity of x & y axes depends on supply voltage:
+ * - 256 LSB/g @ 2.5V
+ * - 265 LSB/g @ 3.3V
+ * z sensitivity stays at 256 LSB/g
+ * fixed point sens: 9.81 [m/s^2] / 256 [LSB/g] * 2^INT32_ACCEL_FRAC
+ * x/y sens = 9.81 / 265 * 1024 = 37.91
+ * z sens = 9.81 / 256 * 1024 = 39.24
+ *
+ * what about the offset at 3.3V?
+ */
+#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
+#define IMU_ACCEL_X_SENS 37.91
+#define IMU_ACCEL_X_SENS_NUM 3791
+#define IMU_ACCEL_X_SENS_DEN 100
+#define IMU_ACCEL_Y_SENS 37.91
+#define IMU_ACCEL_Y_SENS_NUM 3791
+#define IMU_ACCEL_Y_SENS_DEN 100
+#define IMU_ACCEL_Z_SENS 39.24
+#define IMU_ACCEL_Z_SENS_NUM 3924
+#define IMU_ACCEL_Z_SENS_DEN 100
+#endif
+
+#endif /* IMU_GL1_DEFAULTS_H */
diff --git a/sw/airborne/subsystems/imu/imu_nps.h b/sw/airborne/subsystems/imu/imu_nps.h
index 2b7facdc0f..6c1f647da9 100644
--- a/sw/airborne/subsystems/imu/imu_nps.h
+++ b/sw/airborne/subsystems/imu/imu_nps.h
@@ -56,6 +56,19 @@
#endif
+#if !defined IMU_MAG_X_SENS & !defined IMU_MAG_Y_SENS & !defined IMU_MAG_Z_SENS
+#define IMU_MAG_X_SENS 3.5
+#define IMU_MAG_X_SENS_NUM 7
+#define IMU_MAG_X_SENS_DEN 2
+#define IMU_MAG_Y_SENS 3.5
+#define IMU_MAG_Y_SENS_NUM 7
+#define IMU_MAG_Y_SENS_DEN 2
+#define IMU_MAG_Z_SENS 3.5
+#define IMU_MAG_Z_SENS_NUM 7
+#define IMU_MAG_Z_SENS_DEN 2
+#endif
+
+
struct ImuNps {
uint8_t mag_available;
uint8_t accel_available;
diff --git a/sw/airborne/subsystems/imu/imu_px4fmu.c b/sw/airborne/subsystems/imu/imu_px4fmu.c
new file mode 100644
index 0000000000..73be88f475
--- /dev/null
+++ b/sw/airborne/subsystems/imu/imu_px4fmu.c
@@ -0,0 +1,122 @@
+/*
+ * Copyright (C) 2013 Felix Ruess
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file subsystems/imu/imu_px4fmu.c
+ * Driver for the PX4FMU SPI1 for the MPU6000 and I2C2 for the HMC5883.
+ */
+
+#include "subsystems/imu.h"
+
+#include "mcu_periph/spi.h"
+#include "peripherals/hmc58xx_regs.h"
+
+
+/* MPU60x0 gyro/accel internal lowpass frequency */
+#if !defined PX4FMU_LOWPASS_FILTER && !defined PX4FMU_SMPLRT_DIV
+#if (PERIODIC_FREQUENCY == 60) || (PERIODIC_FREQUENCY == 120)
+/* Accelerometer: Bandwidth 44Hz, Delay 4.9ms
+ * Gyroscope: Bandwidth 42Hz, Delay 4.8ms sampling 1kHz
+ */
+#define PX4FMU_LOWPASS_FILTER MPU60X0_DLPF_42HZ
+#define PX4FMU_SMPLRT_DIV 9
+PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz at 1kHz internal sampling")
+#elif PERIODIC_FREQUENCY == 512
+/* Accelerometer: Bandwidth 260Hz, Delay 0ms
+ * Gyroscope: Bandwidth 256Hz, Delay 0.98ms sampling 8kHz
+ */
+#define PX4FMU_LOWPASS_FILTER MPU60X0_DLPF_256HZ
+#define PX4FMU_SMPLRT_DIV 3
+PRINT_CONFIG_MSG("Gyro/Accel output rate is 2kHz at 8kHz internal sampling")
+#else
+#error Non-default PERIODIC_FREQUENCY: please define PX4FMU_LOWPASS_FILTER and PX4FMU_SMPLRT_DIV.
+#endif
+#endif
+PRINT_CONFIG_VAR(PX4FMU_LOWPASS_FILTER)
+PRINT_CONFIG_VAR(PX4FMU_SMPLRT_DIV)
+
+#ifndef PX4FMU_GYRO_RANGE
+#define PX4FMU_GYRO_RANGE MPU60X0_GYRO_RANGE_2000
+#endif
+PRINT_CONFIG_VAR(PX4FMU_GYRO_RANGE)
+
+#ifndef PX4FMU_ACCEL_RANGE
+#define PX4FMU_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G
+#endif
+PRINT_CONFIG_VAR(PX4FMU_ACCEL_RANGE)
+
+struct ImuPx4fmu imu_px4fmu;
+
+void imu_impl_init(void)
+{
+ imu_px4fmu.accel_valid = FALSE;
+ imu_px4fmu.gyro_valid = FALSE;
+ imu_px4fmu.mag_valid = FALSE;
+
+ /* MPU is on spi1 and CS is SLAVE2 */
+ mpu60x0_spi_init(&imu_px4fmu.mpu, &spi1, SPI_SLAVE2);
+ // change the default configuration
+ imu_px4fmu.mpu.config.smplrt_div = PX4FMU_SMPLRT_DIV;
+ imu_px4fmu.mpu.config.dlpf_cfg = PX4FMU_LOWPASS_FILTER;
+ imu_px4fmu.mpu.config.gyro_range = PX4FMU_GYRO_RANGE;
+ imu_px4fmu.mpu.config.accel_range = PX4FMU_ACCEL_RANGE;
+
+ /* initialize mag on i2c2 and set default options */
+ hmc58xx_init(&imu_px4fmu.hmc, &i2c2, HMC58XX_ADDR);
+}
+
+
+void imu_periodic(void)
+{
+ mpu60x0_spi_periodic(&imu_px4fmu.mpu);
+
+ // Read HMC58XX at 50Hz (main loop for rotorcraft: 512Hz)
+ RunOnceEvery(10, hmc58xx_periodic(&imu_px4fmu.hmc));
+}
+
+void imu_px4fmu_event(void)
+{
+ mpu60x0_spi_event(&imu_px4fmu.mpu);
+ if (imu_px4fmu.mpu.data_available) {
+ RATES_ASSIGN(imu.gyro_unscaled,
+ imu_px4fmu.mpu.data_rates.rates.q,
+ imu_px4fmu.mpu.data_rates.rates.p,
+ -imu_px4fmu.mpu.data_rates.rates.r);
+ VECT3_ASSIGN(imu.accel_unscaled,
+ imu_px4fmu.mpu.data_accel.vect.y,
+ imu_px4fmu.mpu.data_accel.vect.x,
+ -imu_px4fmu.mpu.data_accel.vect.z);
+ imu_px4fmu.mpu.data_available = FALSE;
+ imu_px4fmu.gyro_valid = TRUE;
+ imu_px4fmu.accel_valid = TRUE;
+ }
+
+ /* HMC58XX event task */
+ hmc58xx_event(&imu_px4fmu.hmc);
+ if (imu_px4fmu.hmc.data_available) {
+ imu.mag_unscaled.x = imu_px4fmu.hmc.data.vect.y;
+ imu.mag_unscaled.y = imu_px4fmu.hmc.data.vect.x;
+ imu.mag_unscaled.z = -imu_px4fmu.hmc.data.vect.z;
+ imu_px4fmu.hmc.data_available = FALSE;
+ imu_px4fmu.mag_valid = TRUE;
+ }
+}
+
diff --git a/sw/airborne/subsystems/imu/imu_px4fmu.h b/sw/airborne/subsystems/imu/imu_px4fmu.h
new file mode 100644
index 0000000000..1620b9892c
--- /dev/null
+++ b/sw/airborne/subsystems/imu/imu_px4fmu.h
@@ -0,0 +1,83 @@
+/*
+ * Copyright (C) 2013 Felix Ruess
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file subsystems/imu/imu_px4fmu.h
+ * Driver for the PX4FMU SPI1 for the MPU6000 and I2C2 for the HMC5883.
+ */
+
+#ifndef IMU_PX4FMU_SPI_H
+#define IMU_PX4FMU_SPI_H
+
+#include "std.h"
+#include "generated/airframe.h"
+#include "subsystems/imu.h"
+
+#include "subsystems/imu/imu_mpu60x0_defaults.h"
+#include "peripherals/mpu60x0_spi.h"
+#include "peripherals/hmc58xx.h"
+
+#if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN
+#define IMU_GYRO_P_SIGN 1
+#define IMU_GYRO_Q_SIGN 1
+#define IMU_GYRO_R_SIGN 1
+#endif
+#if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN
+#define IMU_ACCEL_X_SIGN 1
+#define IMU_ACCEL_Y_SIGN 1
+#define IMU_ACCEL_Z_SIGN 1
+#endif
+#if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN
+#define IMU_MAG_X_SIGN 1
+#define IMU_MAG_Y_SIGN 1
+#define IMU_MAG_Z_SIGN 1
+#endif
+
+struct ImuPx4fmu {
+ volatile bool_t gyro_valid;
+ volatile bool_t accel_valid;
+ volatile bool_t mag_valid;
+ struct Mpu60x0_Spi mpu;
+ struct Hmc58xx hmc;
+};
+
+extern struct ImuPx4fmu imu_px4fmu;
+
+extern void imu_px4fmu_event(void);
+
+
+static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) {
+ imu_px4fmu_event();
+ if (imu_px4fmu.gyro_valid) {
+ imu_px4fmu.gyro_valid = FALSE;
+ _gyro_handler();
+ }
+ if (imu_px4fmu.accel_valid) {
+ imu_px4fmu.accel_valid = FALSE;
+ _accel_handler();
+ }
+ if (imu_px4fmu.mag_valid) {
+ imu_px4fmu.mag_valid = FALSE;
+ _mag_handler();
+ }
+}
+
+#endif /* IMU_PX4FMU_H */
diff --git a/sw/airborne/subsystems/ins/ins_alt_float.c b/sw/airborne/subsystems/ins/ins_alt_float.c
index b2c78c6f2c..e8727750c7 100644
--- a/sw/airborne/subsystems/ins/ins_alt_float.c
+++ b/sw/airborne/subsystems/ins/ins_alt_float.c
@@ -105,6 +105,10 @@ void ins_update_baro() {
UTM_COPY(utm, *stateGetPositionUtm_f());
utm.alt = ins_alt;
stateSetPositionUtm_f(&utm);
+ struct NedCoor_f ned_vel;
+ memcpy(&ned_vel, stateGetSpeedNed_f(), sizeof(struct NedCoor_f));
+ ned_vel.z = -ins_alt_dot;
+ stateSetSpeedNed_f(&ned_vel);
}
}
#endif
@@ -129,7 +133,7 @@ void ins_update_gps(void) {
struct NedCoor_f ned_vel = {
gps.ned_vel.x / 100.,
gps.ned_vel.y / 100.,
- gps.ned_vel.z / 100.
+ -ins_alt_dot
};
// set velocity
stateSetSpeedNed_f(&ned_vel);
diff --git a/sw/airborne/subsystems/ins/ins_alt_float.h b/sw/airborne/subsystems/ins/ins_alt_float.h
index 2797e679c7..613ebc5281 100644
--- a/sw/airborne/subsystems/ins/ins_alt_float.h
+++ b/sw/airborne/subsystems/ins/ins_alt_float.h
@@ -33,7 +33,7 @@
#include
#include "std.h"
#include "state.h"
-
+#include "generated/modules.h"
#if USE_BAROMETER
#ifdef BARO_MS5534A
diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c
index 7ff6420411..567b21b9f5 100644
--- a/sw/airborne/subsystems/ins/ins_int.c
+++ b/sw/airborne/subsystems/ins/ins_int.c
@@ -72,10 +72,6 @@ struct FloatVect2 ins_gps_speed_m_s_ned;
int32_t ins_qfe;
bool_t ins_baro_initialised;
int32_t ins_baro_alt;
-#if USE_SONAR
-bool_t ins_update_on_agl;
-int32_t ins_sonar_offset;
-#endif
#endif
/* output */
@@ -130,9 +126,6 @@ void ins_init() {
#endif
#if USE_VFF
ins_baro_initialised = FALSE;
-#if USE_SONAR
- ins_update_on_agl = FALSE;
-#endif
vff_init(0., 0., 0.);
#endif
ins.vf_realign = FALSE;
@@ -213,9 +206,6 @@ void ins_update_baro() {
if (ins.vf_realign) {
ins.vf_realign = FALSE;
ins_qfe = baro.absolute;
-#if USE_SONAR
- ins_sonar_offset = sonar_meas;
-#endif
vff_realign(0.);
ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot);
ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot);
@@ -291,13 +281,4 @@ void ins_update_gps(void) {
}
void ins_update_sonar() {
-#if USE_SONAR && 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 && 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 = baro.absolute + (d_sonar * (INS_BARO_SENS_DEN))/INS_BARO_SENS_NUM;
- }
-#endif
}
diff --git a/sw/airborne/subsystems/ins/ins_int.h b/sw/airborne/subsystems/ins/ins_int.h
index dd12b87f45..c72c805c52 100644
--- a/sw/airborne/subsystems/ins/ins_int.h
+++ b/sw/airborne/subsystems/ins/ins_int.h
@@ -49,14 +49,10 @@ extern struct NedCoor_i ins_gps_pos_cm_ned;
extern struct NedCoor_i ins_gps_speed_cm_s_ned;
/* barometer */
-#if USE_VFF || USE_VFF_EXTENDED
+#if USE_VFF
extern int32_t ins_baro_alt; ///< altitude calculated from baro in meters with #INT32_POS_FRAC
extern int32_t ins_qfe;
extern bool_t ins_baro_initialised;
-#if USE_SONAR
-extern bool_t ins_update_on_agl; /* use sonar to update agl if available */
-extern int32_t ins_sonar_offset;
-#endif
#endif
/* output LTP NED */
diff --git a/sw/airborne/subsystems/ins/ins_int_extended.c b/sw/airborne/subsystems/ins/ins_int_extended.c
index 04c0befdb3..aefa07f10f 100644
--- a/sw/airborne/subsystems/ins/ins_int_extended.c
+++ b/sw/airborne/subsystems/ins/ins_int_extended.c
@@ -27,7 +27,7 @@
*
*/
-#include "subsystems/ins/ins_int.h"
+#include "subsystems/ins/ins_int_extended.h"
#include "subsystems/imu.h"
#include "subsystems/sensors/baro.h"
@@ -82,6 +82,7 @@ int32_t ins_baro_alt;
#include "filters/median_filter.h"
struct MedianFilterInt baro_median;
+#if USE_SONAR
/* sonar */
bool_t ins_update_on_agl;
int32_t ins_sonar_alt;
@@ -92,6 +93,7 @@ struct MedianFilterInt sonar_median;
#endif
#define VFF_R_SONAR_0 0.1
#define VFF_R_SONAR_OF_M 0.2
+#endif // USE_SONAR
/* output */
struct NedCoor_i ins_ltp_pos;
@@ -122,9 +124,13 @@ void ins_init() {
ins_baro_initialised = FALSE;
init_median_filter(&baro_median);
+
+#if USE_SONAR
ins_update_on_agl = FALSE;
init_median_filter(&sonar_median);
ins_sonar_offset = INS_SONAR_OFFSET;
+#endif
+
vff_init(0., 0., 0., 0.);
ins.vf_realign = FALSE;
ins.hf_realign = FALSE;
@@ -203,6 +209,7 @@ void ins_update_baro() {
vff_update_baro(alt_float);
}
}
+ INS_NED_TO_STATE();
}
@@ -244,6 +251,7 @@ void ins_update_gps(void) {
#endif /* hff not used */
}
+ INS_NED_TO_STATE();
#endif /* USE_GPS */
}
@@ -261,7 +269,9 @@ float var_err[VAR_ERR_MAX];
uint8_t var_idx = 0;
#endif
+
void ins_update_sonar() {
+#if USE_SONAR
static float last_offset = 0.;
// new value filtered with median_filter
ins_sonar_alt = update_median_filter(&sonar_median, sonar_meas);
@@ -308,4 +318,6 @@ void ins_update_sonar() {
/* update offset with last value to avoid divergence */
vff_update_offset(last_offset);
}
+#endif // USE_SONAR
}
+
diff --git a/sw/airborne/subsystems/ins/ins_int_extended.h b/sw/airborne/subsystems/ins/ins_int_extended.h
new file mode 100644
index 0000000000..6d0ae9b2ec
--- /dev/null
+++ b/sw/airborne/subsystems/ins/ins_int_extended.h
@@ -0,0 +1,82 @@
+/*
+ * Copyright (C) 2008-2012 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.
+ */
+
+/**
+ * @file subsystems/ins/ins_int_extended.h
+ *
+ * INS for rotorcrafts combining vertical and horizontal filters.
+ * This filter includes estimation of the baro drift based on sonar.
+ *
+ * Vertical filter is always enabled
+ *
+ * TODO: use GPS alt if good enough, especially when sonar readings are not available
+ *
+ */
+
+#ifndef INS_INT_EXTENDED_H
+#define INS_INT_EXTENDED_H
+
+#include "subsystems/ins.h"
+#include "std.h"
+#include "math/pprz_geodetic_int.h"
+#include "math/pprz_algebra_float.h"
+
+// TODO integrate all internal state to the structure
+///** Ins extended implementation state (fixed point) */
+//struct InsIntExtended {
+//};
+//
+///** global INS state */
+//extern struct InsInt ins_impl;
+
+/* gps transformed to LTP-NED */
+extern struct LtpDef_i ins_ltp_def;
+extern bool_t ins_ltp_initialised;
+extern struct NedCoor_i ins_gps_pos_cm_ned;
+extern struct NedCoor_i ins_gps_speed_cm_s_ned;
+
+/* barometer */
+extern int32_t ins_baro_alt; ///< altitude calculated from baro in meters with #INT32_POS_FRAC
+extern int32_t ins_qfe;
+extern bool_t ins_baro_initialised;
+#if USE_SONAR
+extern bool_t ins_update_on_agl; /* use sonar to update agl if available */
+extern int32_t ins_sonar_offset;
+#endif
+
+/* output LTP NED */
+extern struct NedCoor_i ins_ltp_pos;
+extern struct NedCoor_i ins_ltp_speed;
+extern struct NedCoor_i ins_ltp_accel;
+#if USE_HFF
+/* horizontal gps transformed to NED in meters as float */
+extern struct FloatVect2 ins_gps_pos_m_ned;
+extern struct FloatVect2 ins_gps_speed_m_s_ned;
+#endif
+
+/* copy position and speed to state interface */
+#define INS_NED_TO_STATE() { \
+ stateSetPositionNed_i(&ins_ltp_pos); \
+ stateSetSpeedNed_i(&ins_ltp_speed); \
+ stateSetAccelNed_i(&ins_ltp_accel); \
+}
+
+#endif /* INS_INT_EXTENDED_H */
diff --git a/sw/airborne/subsystems/ins/vf_extended_float.c b/sw/airborne/subsystems/ins/vf_extended_float.c
index 3fd4bc2cfd..f40b214b7f 100644
--- a/sw/airborne/subsystems/ins/vf_extended_float.c
+++ b/sw/airborne/subsystems/ins/vf_extended_float.c
@@ -30,6 +30,8 @@
*/
#include "subsystems/ins/vf_extended_float.h"
+#include "generated/airframe.h"
+#include "std.h"
#define DEBUG_VFF_EXTENDED 1
@@ -39,6 +41,14 @@
#include "subsystems/datalink/downlink.h"
#endif
+#ifndef INS_PROPAGATE_FREQUENCY
+#define INS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY
+#endif
+PRINT_CONFIG_VAR(INS_PROPAGATE_FREQUENCY)
+
+#define DT_VFILTER (1./(INS_PROPAGATE_FREQUENCY))
+
+
/*
X = [ z zdot accel_bias baro_offset ]
diff --git a/sw/airborne/subsystems/ins/vf_float.c b/sw/airborne/subsystems/ins/vf_float.c
index e49d12a567..a034980456 100644
--- a/sw/airborne/subsystems/ins/vf_float.c
+++ b/sw/airborne/subsystems/ins/vf_float.c
@@ -27,6 +27,15 @@
*/
#include "subsystems/ins/vf_float.h"
+#include "generated/airframe.h"
+#include "std.h"
+
+#ifndef INS_PROPAGATE_FREQUENCY
+#define INS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY
+#endif
+PRINT_CONFIG_VAR(INS_PROPAGATE_FREQUENCY)
+
+#define DT_VFILTER (1./(INS_PROPAGATE_FREQUENCY))
/*
diff --git a/sw/airborne/subsystems/navigation/common_nav.c b/sw/airborne/subsystems/navigation/common_nav.c
index 317fa475b3..7e641c05d6 100644
--- a/sw/airborne/subsystems/navigation/common_nav.c
+++ b/sw/airborne/subsystems/navigation/common_nav.c
@@ -61,6 +61,36 @@ void compute_dist2_to_home(void) {
static float previous_ground_alt;
+/** Reset the UTM zone to current GPS fix */
+unit_t nav_reset_utm_zone(void) {
+
+ struct UtmCoor_f utm0_old;
+ utm0_old.zone = nav_utm_zone0;
+ utm0_old.north = nav_utm_north0;
+ utm0_old.east = nav_utm_east0;
+ utm0_old.alt = ground_alt;
+ struct LlaCoor_f lla0;
+ lla_of_utm_f(&lla0, &utm0_old);
+
+#ifdef GPS_USE_LATLONG
+ /* Set the real UTM zone */
+ nav_utm_zone0 = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
+#else
+ nav_utm_zone0 = gps.utm_pos.zone;
+#endif
+
+ struct UtmCoor_f utm0;
+ utm0.zone = nav_utm_zone0;
+ utm_of_lla_f(&utm0, &lla0);
+
+ nav_utm_east0 = utm0.east;
+ nav_utm_north0 = utm0.north;
+
+ stateSetLocalUtmOrigin_f(&utm0);
+
+ return 0;
+}
+
/** Reset the geographic reference to the current GPS fix */
unit_t nav_reset_reference( void ) {
#ifdef GPS_USE_LATLONG
diff --git a/sw/airborne/subsystems/navigation/common_nav.h b/sw/airborne/subsystems/navigation/common_nav.h
index c7f0343489..3698d92702 100644
--- a/sw/airborne/subsystems/navigation/common_nav.h
+++ b/sw/airborne/subsystems/navigation/common_nav.h
@@ -61,6 +61,7 @@ extern uint8_t nav_utm_zone0;
void compute_dist2_to_home(void);
+unit_t nav_reset_utm_zone(void);
unit_t nav_reset_reference( void ) __attribute__ ((unused));
unit_t nav_update_waypoints_alt( void ) __attribute__ ((unused));
void common_nav_periodic_task_4Hz(void);
diff --git a/sw/airborne/subsystems/radio_control/sbus.c b/sw/airborne/subsystems/radio_control/sbus.c
index e5e9f704db..8ecdaa6c60 100644
--- a/sw/airborne/subsystems/radio_control/sbus.c
+++ b/sw/airborne/subsystems/radio_control/sbus.c
@@ -52,7 +52,7 @@
* output low sets it to normal polarity.
*/
#ifndef RC_SET_POLARITY
-#define RC_SET_POLARITY gpio_output_high
+#define RC_SET_POLARITY gpio_set
#endif
diff --git a/sw/airborne/subsystems/radio_control/superbitrf_rc.c b/sw/airborne/subsystems/radio_control/superbitrf_rc.c
new file mode 100644
index 0000000000..f3609ed3b8
--- /dev/null
+++ b/sw/airborne/subsystems/radio_control/superbitrf_rc.c
@@ -0,0 +1,37 @@
+/*
+ * Copyright (C) 2013 Freek van Tienen
+ *
+ * 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 subsystems/radio_control/superbitrf_rc.c
+ * DSM2 and DSMX radio control implementation for the cyrf6936 2.4GHz radio chip trough SPI
+ */
+
+#include "superbitrf_rc.h"
+#include "subsystems/radio_control.h"
+
+/**
+ * Initialization
+ */
+//#if DATALINK == SUPERBITRF
+//void radio_control_impl_init(void) {}
+//#else
+void radio_control_impl_init(void) { superbitrf_init(); }
+//#endif
diff --git a/sw/airborne/subsystems/radio_control/superbitrf_rc.h b/sw/airborne/subsystems/radio_control/superbitrf_rc.h
new file mode 100644
index 0000000000..de410a56d3
--- /dev/null
+++ b/sw/airborne/subsystems/radio_control/superbitrf_rc.h
@@ -0,0 +1,89 @@
+/*
+ * Copyright (C) 2013 Freek van Tienen
+ *
+ * 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 subsystems/radio_control/superbitrf_rc.h
+ * DSM2 and DSMX radio control implementation for the cyrf6936 2.4GHz radio chip trough SPI
+ */
+
+#ifndef RADIO_CONTROL_SUPERBITRF_RC_H
+#define RADIO_CONTROL_SUPERBITRF_RC_H
+
+#include "subsystems/datalink/superbitrf.h"
+
+/* Theoretically you could have 14 channel over DSM2/DSMX */
+#ifndef RADIO_CONTROL_NB_CHANNEL
+#define RADIO_CONTROL_NB_CHANNEL 14
+#endif
+
+/* The channel ordering is always the same for DSM2 and DSMX */
+#define RADIO_THROTTLE 0
+#define RADIO_ROLL 1
+#define RADIO_PITCH 2
+#define RADIO_YAW 3
+#define RADIO_GEAR 4
+#define RADIO_FLAP 5
+#define RADIO_AUX1 5
+#define RADIO_AUX2 6
+#define RADIO_AUX3 7
+#define RADIO_AUX4 8
+#define RADIO_AUX5 9
+#define RADIO_AUX6 10
+#define RADIO_AUX7 11
+#define RADIO_AUX8 12
+#define RADIO_AUX9 13
+
+/* Map the MODE default to the gear switch */
+#ifndef RADIO_MODE
+#define RADIO_MODE RADIO_GEAR
+#endif
+
+/* Macro that normalize superbitrf rc_values to radio values */
+#define NormalizeRcDl(_in, _out, _count) { \
+ uint8_t i; \
+ for(i = 0; i < _count; i++) { \
+ if(i == RADIO_THROTTLE) { \
+ _out[i] = (_in[i] + MAX_PPRZ) / 2; \
+ Bound(_out[i], 0, MAX_PPRZ); \
+ } else { \
+ _out[i] = -_in[i]; \
+ Bound(_out[i], MIN_PPRZ, MAX_PPRZ); \
+ } \
+ } \
+}
+
+/* The radio control event handler */
+#define RadioControlEvent(_received_frame_handler) { \
+ cyrf6936_event(&superbitrf.cyrf6936); \
+ superbitrf_event(); \
+ if(superbitrf.rc_frame_available) { \
+ radio_control.frame_cpt++; \
+ radio_control.time_since_last_frame = 0; \
+ radio_control.radio_ok_cpt = 0; \
+ radio_control.status = RC_OK; \
+ NormalizeRcDl(superbitrf.rc_values,radio_control.values \
+ ,superbitrf.num_channels); \
+ _received_frame_handler(); \
+ superbitrf.rc_frame_available = FALSE; \
+ } \
+}
+
+#endif /* RADIO_CONTROL_SUPERBITRF_RC_H */
diff --git a/sw/airborne/subsystems/sensors/baro_ms5611_i2c.c b/sw/airborne/subsystems/sensors/baro_ms5611_i2c.c
new file mode 100644
index 0000000000..a3c446e4bd
--- /dev/null
+++ b/sw/airborne/subsystems/sensors/baro_ms5611_i2c.c
@@ -0,0 +1,120 @@
+/*
+ * Copyright (C) 2011-2013 The Paparazzi Team
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/**
+ * @file subsystems/sensors/baro_ms5611_i2c.c
+ *
+ * Driver for MS5611 baro via I2C.
+ *
+ */
+
+#include "subsystems/sensors/baro.h"
+#include "peripherals/ms5611_i2c.h"
+
+#include "mcu_periph/sys_time.h"
+#include "led.h"
+#include "std.h"
+
+#ifndef MS5611_I2C_DEV
+#define MS5611_I2C_DEV i2c2
+#endif
+
+/* default i2c address
+ * when CSB is set to GND addr is 0xEE
+ * when CSB is set to VCC addr is 0xEC
+ *
+ * Note: Aspirin 2.1 has CSB bound to GND.
+ */
+#ifndef MS5611_SLAVE_ADDR
+#define MS5611_SLAVE_ADDR 0xEE
+#endif
+
+#ifdef DEBUG
+
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+#include "mcu_periph/uart.h"
+#include "messages.h"
+#include "subsystems/datalink/downlink.h"
+
+float fbaroms, ftempms;
+#endif
+
+struct Baro baro;
+struct Ms5611_I2c baro_ms5611;
+
+
+void baro_init(void) {
+ baro.status = BS_UNINITIALIZED;
+ baro.absolute = 0;
+ baro.differential = 0;
+
+ ms5611_i2c_init(&baro_ms5611, &MS5611_I2C_DEV, MS5611_SLAVE_ADDR);
+}
+
+void baro_periodic(void) {
+ if (sys_time.nb_sec > 1) {
+
+ /* call the convenience periodic that initializes the sensor and starts reading*/
+ ms5611_i2c_periodic(&baro_ms5611);
+
+ if (baro_ms5611.initialized) {
+ baro.status = BS_RUNNING;
+#if DEBUG
+ RunOnceEvery((4*30), DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice,
+ &baro_ms5611.data.c[0],
+ &baro_ms5611.data.c[1],
+ &baro_ms5611.data.c[2],
+ &baro_ms5611.data.c[3],
+ &baro_ms5611.data.c[4],
+ &baro_ms5611.data.c[5],
+ &baro_ms5611.data.c[6],
+ &baro_ms5611.data.c[7]));
+#endif
+ }
+ }
+}
+
+void baro_event(void (*b_abs_handler)(void)){
+ if (sys_time.nb_sec > 1) {
+ ms5611_i2c_event(&baro_ms5611);
+
+ if (baro_ms5611.data_available) {
+ baro.absolute = baro_ms5611.data.pressure;
+ b_abs_handler();
+ baro_ms5611.data_available = FALSE;
+
+#ifdef ROTORCRAFT_BARO_LED
+ RunOnceEvery(10,LED_TOGGLE(ROTORCRAFT_BARO_LED));
+#endif
+
+#if DEBUG
+ ftempms = baro_ms5611.data.temperature / 100.;
+ fbaroms = baro_ms5611.data.pressure / 100.;
+ DOWNLINK_SEND_BARO_MS5611(DefaultChannel, DefaultDevice,
+ &baro_ms5611.data.d1, &baro_ms5611.data.d2,
+ &fbaroms, &ftempms);
+#endif
+ }
+ }
+}
diff --git a/sw/airborne/subsystems/sensors/baro_ms5611_spi.c b/sw/airborne/subsystems/sensors/baro_ms5611_spi.c
new file mode 100644
index 0000000000..bd8603ea43
--- /dev/null
+++ b/sw/airborne/subsystems/sensors/baro_ms5611_spi.c
@@ -0,0 +1,118 @@
+/*
+ * Copyright (C) 2011-2013 The Paparazzi Team
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/**
+ * @file subsystems/sensors/baro_ms5611_spi.c
+ *
+ * Driver for MS5611 baro on LisaM/Aspirin2.2 via SPI.
+ *
+ */
+
+#include "subsystems/sensors/baro.h"
+#include "peripherals/ms5611_spi.h"
+
+#include "mcu_periph/sys_time.h"
+#include "led.h"
+#include "std.h"
+
+#ifndef MS5611_SPI_DEV
+#define MS5611_SPI_DEV spi2
+#endif
+
+/* SPI SLAVE3 is on pin PC13
+ * Aspirin 2.2 has ms5611 on SPI bus
+ */
+#ifndef MS5611_SLAVE_DEV
+#define MS5611_SLAVE_DEV SPI_SLAVE3
+#endif
+
+
+#ifdef DEBUG
+
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+#include "mcu_periph/uart.h"
+#include "messages.h"
+#include "subsystems/datalink/downlink.h"
+
+float fbaroms, ftempms;
+#endif
+
+struct Baro baro;
+struct Ms5611_Spi baro_ms5611;
+
+
+void baro_init(void) {
+ baro.status = BS_UNINITIALIZED;
+ baro.absolute = 0;
+ baro.differential = 0;
+
+ ms5611_spi_init(&baro_ms5611, &MS5611_SPI_DEV, MS5611_SLAVE_DEV);
+}
+
+void baro_periodic(void) {
+ if (sys_time.nb_sec > 1) {
+
+ /* call the convenience periodic that initializes the sensor and starts reading*/
+ ms5611_spi_periodic(&baro_ms5611);
+
+ if (baro_ms5611.initialized) {
+ baro.status = BS_RUNNING;
+#if DEBUG
+ RunOnceEvery((4*30), DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice,
+ &baro_ms5611.data.c[0],
+ &baro_ms5611.data.c[1],
+ &baro_ms5611.data.c[2],
+ &baro_ms5611.data.c[3],
+ &baro_ms5611.data.c[4],
+ &baro_ms5611.data.c[5],
+ &baro_ms5611.data.c[6],
+ &baro_ms5611.data.c[7]));
+#endif
+ }
+ }
+}
+
+void baro_event(void (*b_abs_handler)(void)){
+ if (sys_time.nb_sec > 1) {
+ ms5611_spi_event(&baro_ms5611);
+
+ if (baro_ms5611.data_available) {
+ baro.absolute = baro_ms5611.data.pressure;
+ b_abs_handler();
+ baro_ms5611.data_available = FALSE;
+
+#ifdef ROTORCRAFT_BARO_LED
+ RunOnceEvery(10,LED_TOGGLE(ROTORCRAFT_BARO_LED));
+#endif
+
+#if DEBUG
+ ftempms = baro_ms5611.data.temperature / 100.;
+ fbaroms = baro_ms5611.data.pressure / 100.;
+ DOWNLINK_SEND_BARO_MS5611(DefaultChannel, DefaultDevice,
+ &baro_ms5611.data.d1, &baro_ms5611.data.d2,
+ &fbaroms, &ftempms);
+#endif
+ }
+ }
+}
diff --git a/sw/airborne/subsystems/sonar.h b/sw/airborne/subsystems/sonar.h
new file mode 100644
index 0000000000..daf310400d
--- /dev/null
+++ b/sw/airborne/subsystems/sonar.h
@@ -0,0 +1,4 @@
+
+
+
+extern uint16_t sonar_meas;
diff --git a/sw/airborne/test/subsystems/test_ahrs.c b/sw/airborne/test/subsystems/test_ahrs.c
index 3871888bf6..701fc0bda5 100644
--- a/sw/airborne/test/subsystems/test_ahrs.c
+++ b/sw/airborne/test/subsystems/test_ahrs.c
@@ -159,6 +159,7 @@ static inline void main_report(void) {
},
{
#ifdef USE_I2C2
+ uint16_t i2c2_queue_full_cnt = i2c2.errors->queue_full_cnt;
uint16_t i2c2_ack_fail_cnt = i2c2.errors->ack_fail_cnt;
uint16_t i2c2_miss_start_stop_cnt = i2c2.errors->miss_start_stop_cnt;
uint16_t i2c2_arb_lost_cnt = i2c2.errors->arb_lost_cnt;
@@ -170,6 +171,7 @@ static inline void main_report(void) {
uint32_t i2c2_last_unexpected_event = i2c2.errors->last_unexpected_event;
const uint8_t _bus2 = 2;
DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice,
+ &i2c2_queue_full_cnt,
&i2c2_ack_fail_cnt,
&i2c2_miss_start_stop_cnt,
&i2c2_arb_lost_cnt,
diff --git a/sw/airborne/test/subsystems/test_imu.c b/sw/airborne/test/subsystems/test_imu.c
index 332cbba003..e3bb239df2 100644
--- a/sw/airborne/test/subsystems/test_imu.c
+++ b/sw/airborne/test/subsystems/test_imu.c
@@ -80,6 +80,7 @@ static inline void main_periodic_task( void ) {
});
#ifdef USE_I2C2
RunOnceEvery(111, {
+ uint16_t i2c2_queue_full_cnt = i2c2.errors->queue_full_cnt;
uint16_t i2c2_ack_fail_cnt = i2c2.errors->ack_fail_cnt;
uint16_t i2c2_miss_start_stop_cnt = i2c2.errors->miss_start_stop_cnt;
uint16_t i2c2_arb_lost_cnt = i2c2.errors->arb_lost_cnt;
@@ -91,6 +92,7 @@ static inline void main_periodic_task( void ) {
uint32_t i2c2_last_unexpected_event = i2c2.errors->last_unexpected_event;
const uint8_t _bus2 = 2;
DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice,
+ &i2c2_queue_full_cnt,
&i2c2_ack_fail_cnt,
&i2c2_miss_start_stop_cnt,
&i2c2_arb_lost_cnt,
diff --git a/sw/ext/ardrone2_drivers/cdc-acm.ko b/sw/ext/ardrone2_drivers/cdc-acm.ko
index b8db911440..9270b2795f 100644
Binary files a/sw/ext/ardrone2_drivers/cdc-acm.ko and b/sw/ext/ardrone2_drivers/cdc-acm.ko differ
diff --git a/sw/ext/ardrone2_drivers/check_update.sh b/sw/ext/ardrone2_drivers/check_update.sh
new file mode 100755
index 0000000000..3fee525f48
--- /dev/null
+++ b/sw/ext/ardrone2_drivers/check_update.sh
@@ -0,0 +1,96 @@
+#!/bin/sh
+
+UPDATE_DIR=/update
+UPDATE_PATH=$UPDATE_DIR/ardrone2_update.plf
+VERSION_PATH=$UPDATE_DIR/version.txt
+ERR_PATH=$UPDATE_DIR/err.log
+
+echo "Check if need to start paparazzi ..."
+START_PAPARAZZI=`grep start_paparazzi /data/config.ini | awk -F "=" '{ gsub(/ */,"",$2); print $2}'`
+case $START_PAPARAZZI in
+1)
+ START_PAPARAZZI=raw
+ ;;
+2)
+ START_PAPARAZZI=sdk
+ ;;
+*)
+ START_PAPARAZZI=no
+ ;;
+esac
+echo "START_PAPARAZZI=$START_PAPARAZZI"
+
+echo "Copy version.txt file in ftp directory"
+cp /firmware/version.txt $VERSION_PATH
+
+PELF_ARGS=$(cat /tmp/.program.elf.arguments | tr '\n' ' ')
+
+echo "Check if update is necessary ..."
+if [ -e $UPDATE_PATH ] ; then
+ VERSION=`cat $VERSION_PATH`
+
+ if [ -e $ERR_PATH ] ; then
+ CHECK_ERR=`cat $ERR_PATH`
+ if [ "$CHECK_ERR" = "NEED_TO_FLASH" ] ; then
+ CHECK_PLF=`/bin/checkplf $UPDATE_PATH $VERSION`
+ if [ "$CHECK_PLF" = "NEED_TO_FLASH" ] ; then
+ echo "ERR=FLASH_KO" > $ERR_PATH
+ else
+ /bin/checkplf $UPDATE_PATH $VERSION > $ERR_PATH
+ fi
+ else
+ /bin/checkplf $UPDATE_PATH $VERSION > $ERR_PATH
+ fi
+ else
+ /bin/checkplf $UPDATE_PATH $VERSION > $ERR_PATH
+ fi
+
+ CHECK_ERR=`cat $ERR_PATH`
+ if [ "$CHECK_ERR" = "NEED_TO_FLASH" ] ; then
+ echo "File $UPDATE_PATH exists... Start updating..."
+ pinst_trigger
+ echo "Rebooting..."
+ reboot
+ else
+ if [ "$CHECK_ERR" = "VERSION_OK" ] ; then
+ echo "Version OK"
+ elif [ "$CHECK_ERR" = "ERR=FLASH_KO" ] ; then
+ echo "Error during Updating... Removing..."
+ else
+ echo "File $UPDATE_PATH not valid... Removing..."
+ fi
+# Cleaning update directory
+ rm -Rf $UPDATE_DIR/*
+ cp /firmware/version.txt $VERSION_PATH
+ echo "Start Drone software..."
+ inetd
+
+ # Check what to start
+ if [ "$START_PAPARAZZI" = "raw" ] ; then
+ (/data/video/raw/ap.elf; gpio 181 -d ho 1) &
+ elif [ "$START_PAPARAZZI" = "sdk" ] ; then
+ (/data/video/sdk/ap.elf; gpio 181 -d ho 1) &
+ else
+ (/bin/program.elf ${PELF_ARGS}; gpio 181 -d ho 1) &
+ fi
+ fi
+else
+ echo "File $UPDATE_PATH doesn't exists... Start Drone software..."
+# Cleaning update directory
+ rm -Rf $UPDATE_DIR/*
+ cp /firmware/version.txt $VERSION_PATH
+
+ inetd
+ # Check what to start
+ if [ "$START_PAPARAZZI" = "raw" ] ; then
+ (/bin/program.elf ${PELF_ARGS}; gpio 181 -d ho 1) &
+ sleep 10
+ (/data/video/raw/ap.elf; gpio 181 -d ho 1) &
+ elif [ "$START_PAPARAZZI" = "sdk" ] ; then
+ (/bin/program.elf ${PELF_ARGS}; gpio 181 -d ho 1) &
+ sleep 10
+ (/data/video/sdk/ap.elf; gpio 181 -d ho 1) &
+ else
+ (/bin/program.elf ${PELF_ARGS}; gpio 181 -d ho 1) &
+ fi
+fi
diff --git a/sw/ext/ardrone2_drivers/cp210x.ko b/sw/ext/ardrone2_drivers/cp210x.ko
deleted file mode 100644
index 0f811d1807..0000000000
Binary files a/sw/ext/ardrone2_drivers/cp210x.ko and /dev/null differ
diff --git a/sw/ext/ardrone2_drivers/ftdi-sio.ko b/sw/ext/ardrone2_drivers/ftdi-sio.ko
deleted file mode 100644
index 1553306fb9..0000000000
Binary files a/sw/ext/ardrone2_drivers/ftdi-sio.ko and /dev/null differ
diff --git a/sw/ext/ardrone2_drivers/pl2303.ko b/sw/ext/ardrone2_drivers/pl2303.ko
deleted file mode 100644
index 4d61b32426..0000000000
Binary files a/sw/ext/ardrone2_drivers/pl2303.ko and /dev/null differ
diff --git a/sw/ext/ardrone2_drivers/usbserial.ko b/sw/ext/ardrone2_drivers/usbserial.ko
deleted file mode 100644
index cae655f71b..0000000000
Binary files a/sw/ext/ardrone2_drivers/usbserial.ko and /dev/null differ
diff --git a/sw/ext/ardrone2_drivers/wifi_setup.sh b/sw/ext/ardrone2_drivers/wifi_setup.sh
new file mode 100755
index 0000000000..a1e075d98d
--- /dev/null
+++ b/sw/ext/ardrone2_drivers/wifi_setup.sh
@@ -0,0 +1,201 @@
+#!/bin/sh
+#
+# Script to see if an IP adress is already used or not
+#
+# Getting SSID from config.ini file.
+
+#initializing random generator
+cat /data/random_mac.txt > /dev/urandom
+/bin/random_mac > /data/random_mac.txt
+
+#echo 2 > /proc/cpu/alignment
+
+export NETIF=ath0
+export WORKAREA="/lib/firmware"
+export ATH_PLATFORM="parrot-omap-sdio"
+export ATH_MODULE_ARGS="ifname=$NETIF"
+
+# Switch Wifi mode depending on value into /data/config.ini
+#
+
+WIFI_MODE=`grep wifi_mode /data/config.ini | awk -F "=" '{ gsub(/ */,"",$2); print $2}'`
+
+case $WIFI_MODE in
+0)
+ WIFI_MODE=master
+ ;;
+1)
+ WIFI_MODE=ad-hoc
+ ;;
+2)
+ WIFI_MODE=managed
+ ;;
+*)
+ WIFI_MODE=master
+ ;;
+esac
+
+
+if [ -s /factory/mac_address.txt ]
+then
+MAC_ADDR=`cat /factory/mac_address.txt`
+else
+MAC_ADDR=`cat /data/random_mac.txt`
+fi
+
+loadAR6000.sh -i $NETIF --setmac $MAC_ADDR
+
+# Waiting 2s for the wifi chip to be ready
+sleep 2
+
+AR6K_PID=`ps_procps -A -T -c | grep AR6K | awk '{print $1}'`
+SDIO_PID=`ps_procps -A -T -c | grep ksdioirqd | awk '{print $1}'`
+
+#Changing wifi priority
+chrt -p -r 25 $SDIO_PID
+chrt -p -r 24 $AR6K_PID
+
+
+# Disabling powersaving
+wmiconfig -i $NETIF --power maxperf
+# Disabling 802.11n aggregation
+wmiconfig -i $NETIF --allow_aggr 0 0
+# enabling WMM
+wmiconfig -i $NETIF --setwmm 1
+
+i=0
+
+while [ ! -n "$SSID" ] && [ $i -lt 10 ]
+do
+i=`expr $i + 1`
+SSID=`grep ssid_single_player /data/config.ini | awk -F "=" '{print $2}'`
+
+# Removing leading and trailing spaces
+SSID=`echo $SSID`
+sleep 1
+done
+
+if [ -n "$SSID" ]
+then
+echo "SSID=$SSID"
+else
+#default SSID.
+SSID=ardrone2_wifi
+echo "SSID=\"$SSID\""
+fi
+
+RANDOM_CHAN=auto
+
+
+echo "Creating $WIFI_MODE Network $SSID"
+
+iwconfig $NETIF mode $WIFI_MODE
+iwconfig $NETIF essid "$SSID"
+
+if [ "$WIFI_MODE" != "managed" ]
+then
+# Allowing ACS to select only channels 1 & 6
+wmiconfig -i $NETIF --acsdisablehichannels 1
+iwconfig $NETIF channel $RANDOM_CHAN
+iwconfig $NETIF rate auto
+iwconfig $NETIF commit
+else
+# The Wifi connection freezes when in managed mode if there is no keepalive
+wmiconfig -i ath0 --setkeepalive 1
+fi
+
+wmiconfig -i $NETIF --dtim 1
+wmiconfig -i $NETIF --commit
+
+OK=0
+BASE_ADRESS=`grep static_ip_address_base /data/config.ini | awk -F "=" '{print $2}'`
+PROBE=`grep static_ip_address_probe /data/config.ini | awk -F "=" '{print $2}'`
+
+# Removing leading and trailing spaces
+BASE_ADRESS=`echo $BASE_ADRESS`
+PROBE=`echo $PROBE`
+
+# Default base address
+if [ -n "$BASE_ADRESS" ]
+then
+echo "BASE_ADRESS=$BASE_ADRESS"
+else
+#default BASE_ADDRESS.
+BASE_ADRESS=192.168.1.
+echo "BASE_ADRESS=\"$BASE_ADRESS\""
+fi
+
+# Default probe
+if [ -n "$PROBE" ]
+then
+echo "PROBE=$PROBE"
+else
+#default PROBE.
+PROBE=1
+echo "PROBE=\"$PROBE\""
+fi
+
+while [ $OK -eq 0 ]
+do
+#configuring interface.
+ifconfig $NETIF $BASE_ADRESS$PROBE
+arping -I $NETIF -q -f -D -w 2 $BASE_ADRESS$PROBE
+
+if [ $? -eq 1 ]
+then
+ if [ -s /data/old_adress.txt ]
+ then
+ # Testing previously given adress.
+ PROBE=`cat /data/old_adress.txt`
+ else
+ #generating random odd IP address
+ PROBE=`/bin/random_ip`
+ fi
+ /bin/random_ip > /data/old_adress.txt
+else
+ echo $PROBE > /data/old_adress.txt
+ OK=1
+fi
+
+done
+
+#Configuring DHCP server.
+echo "Using address $BASE_ADRESS$PROBE"
+echo "start $BASE_ADRESS`expr $PROBE + 1`" > /tmp/udhcpd.conf
+echo "end $BASE_ADRESS`expr $PROBE + 4`" >> /tmp/udhcpd.conf
+echo "interface $NETIF" >> /tmp/udhcpd.conf
+echo "decline_time 1" >> /tmp/udhcpd.conf
+echo "conflict_time 1" >> /tmp/udhcpd.conf
+echo "opt router $BASE_ADRESS$PROBE" >> /tmp/udhcpd.conf
+echo "opt subnet 255.255.255.0" >> /tmp/udhcpd.conf
+echo "opt lease 1200" >> /tmp/udhcpd.conf
+
+/bin/pairing_setup.sh
+
+# Saving random info for initialization at next reboot
+echo $MAC_ADDR `date` `/bin/random_mac` > /dev/urandom
+/bin/random_mac > /data/random_mac.txt
+
+
+
+telnetd -l /bin/sh
+
+# Check if not booting in master mode
+if [ "$WIFI_MODE" != "managed" ]
+then
+ udhcpd /tmp/udhcpd.conf
+fi
+
+# Adding route for multicast-packet
+route add -net 224.0.0.0 netmask 240.0.0.0 dev $NETIF
+
+# Allow reconnection to dirty TCP ports
+echo "1" > /proc/sys/net/ipv4/tcp_tw_reuse
+echo "1" > /proc/sys/net/ipv4/tcp_tw_recycle
+
+# Starting the daemon which responds to the AUTH messages from FreeFlight if not in master mode
+if [ "$WIFI_MODE" != "managed" ]
+then
+ /bin/parrotauthdaemon
+fi
+
diff --git a/sw/ground_segment/cockpit/editFP.ml b/sw/ground_segment/cockpit/editFP.ml
index 6b87f0c156..3a3673736f 100644
--- a/sw/ground_segment/cockpit/editFP.ml
+++ b/sw/ground_segment/cockpit/editFP.ml
@@ -50,7 +50,9 @@ let save_fp = fun geomap ->
None -> ()
| Some file ->
let f = open_out file in
- fprintf f "\n\n";
+ let fp_path = Str.replace_first (Str.regexp Env.flight_plans_path) "" (Filename.dirname file) in
+ let rel_path = Str.global_replace (Str.regexp (Printf.sprintf "%s[^%s]+" Filename.dir_sep Filename.dir_sep)) (Filename.parent_dir_name // "") fp_path in
+ fprintf f "\n\n" rel_path "flight_plan.dtd";
fprintf f "%s\n" (ExtXml.to_string_fmt fp#xml);
close_out f;
current_fp := Some (fp, file);
diff --git a/sw/lib/ocaml/maps_support.ml b/sw/lib/ocaml/maps_support.ml
index b6aacd5807..9d15ffe48c 100644
--- a/sw/lib/ocaml/maps_support.ml
+++ b/sw/lib/ocaml/maps_support.ml
@@ -24,9 +24,10 @@
let home = Env.paparazzi_home
let (//) = Filename.concat
let maps_xml_path = home // "conf" // "maps.xml"
+let maps_xml_default_path = home // "conf" // "maps_example.xml"
let maps_xml = ExtXml.parse_file maps_xml_path
-let maps_xml_default = ExtXml.parse_file (maps_xml_path^".example")
+let maps_xml_default = ExtXml.parse_file (maps_xml_default_path)
let gv = try Some (ExtXml.int_attrib maps_xml "google_version") with _ -> None
let gv_default = try ExtXml.int_attrib maps_xml_default "google_version" with _ -> 0
diff --git a/sw/simulator/nps/nps_atmosphere.c b/sw/simulator/nps/nps_atmosphere.c
new file mode 100644
index 0000000000..d3aba4bfcf
--- /dev/null
+++ b/sw/simulator/nps/nps_atmosphere.c
@@ -0,0 +1,61 @@
+/*
+ * Copyright (C) 2013 Felix Ruess
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file nps_atmosphere.c
+ * Atmosphere model (pressure, wind) for NPS.
+ */
+
+#include "nps_atmosphere.h"
+#include "nps_fdm.h"
+
+#ifndef NPS_QNH
+#define NPS_QNH 101325.0
+#endif
+
+#ifndef NPS_WIND_SPEED
+#define NPS_WIND_SPEED 0.0
+#endif
+
+#ifndef NPS_WIND_DIR
+#define NPS_WIND_DIR 0
+#endif
+
+#ifndef NPS_TURBULENCE_SEVERITY
+#define NPS_TURBULENCE_SEVERITY 0
+#endif
+
+struct NpsAtmosphere nps_atmosphere;
+
+void nps_atmosphere_init(void) {
+
+ nps_atmosphere.qnh = NPS_QNH;
+ nps_atmosphere.wind_speed = NPS_WIND_SPEED;
+ nps_atmosphere.wind_dir = NPS_WIND_DIR;
+ nps_atmosphere.turbulence_severity = NPS_TURBULENCE_SEVERITY;
+
+}
+
+void nps_atmosphere_update(double dt __attribute__((unused))) {
+ nps_fdm_set_wind(nps_atmosphere.wind_speed, nps_atmosphere.wind_dir,
+ nps_atmosphere.turbulence_severity);
+}
+
diff --git a/sw/simulator/nps/nps_atmosphere.h b/sw/simulator/nps/nps_atmosphere.h
index d167c77dff..683db7e974 100644
--- a/sw/simulator/nps/nps_atmosphere.h
+++ b/sw/simulator/nps/nps_atmosphere.h
@@ -1,6 +1,46 @@
+/*
+ * Copyright (C) 2013 Felix Ruess
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file nps_atmosphere.h
+ * Atmosphere model (pressure, wind) for NPS.
+ */
+
#ifndef NPS_ATMOSPHERE_H
#define NPS_ATMOSPHERE_H
+#include "math/pprz_algebra_double.h"
+
+struct NpsAtmosphere {
+ double qnh; ///< barometric pressure at sea level in Pascal
+ double wind_speed; ///< wind magnitude in m/s
+ double wind_dir; ///< wind direction in radians north=0, increasing CCW
+ int turbulence_severity; ///< turbulence severity from 0-7
+};
+
+extern struct NpsAtmosphere nps_atmosphere;
+
+extern void nps_atmosphere_init(void);
+extern void nps_atmosphere_update(double dt);
+
#endif /* NPS_ATMOSPHERE_H */
diff --git a/sw/simulator/nps/nps_autopilot_fixedwing.c b/sw/simulator/nps/nps_autopilot_fixedwing.c
new file mode 100644
index 0000000000..27826df4e3
--- /dev/null
+++ b/sw/simulator/nps/nps_autopilot_fixedwing.c
@@ -0,0 +1,143 @@
+/*
+ * Copyright (C) 2009 Antoine Drouin
+ * Copyright (C) 2013 The Paparazzi Team
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+#include "nps_autopilot_fixedwing.h"
+
+#ifdef FBW
+#include "firmwares/fixedwing/main_fbw.h"
+#define Fbw(f) f ## _fbw()
+#else
+#define Fbw(f)
+#endif
+
+#ifdef AP
+#include "firmwares/fixedwing/main_ap.h"
+#define Ap(f) f ## _ap()
+#else
+#define Ap(f)
+#endif
+
+#include "nps_sensors.h"
+#include "nps_radio_control.h"
+#include "nps_electrical.h"
+#include "nps_fdm.h"
+
+#include "subsystems/radio_control.h"
+#include "subsystems/imu.h"
+#include "subsystems/sensors/baro.h"
+#include "baro_board.h"
+#include "mcu_periph/sys_time.h"
+#include "state.h"
+#include "subsystems/commands.h"
+
+
+
+struct NpsAutopilot autopilot;
+bool_t nps_bypass_ahrs;
+
+#ifndef NPS_BYPASS_AHRS
+#define NPS_BYPASS_AHRS FALSE
+#endif
+
+#if !defined (FBW) || !defined (AP)
+#error NPS does not currently support dual processor simulation for FBW and AP on fixedwing!
+#endif
+
+void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, char* rc_dev) {
+
+ nps_radio_control_init(type_rc, num_rc_script, rc_dev);
+ nps_electrical_init();
+
+ nps_bypass_ahrs = NPS_BYPASS_AHRS;
+
+ Fbw(init);
+ Ap(init);
+
+}
+
+void nps_autopilot_run_systime_step( void ) {
+ sys_tick_handler();
+}
+
+#include
+#include "subsystems/gps.h"
+
+void nps_autopilot_run_step(double time) {
+
+ nps_electrical_run_step(time);
+
+#ifdef RADIO_CONTROL_TYPE_PPM
+ if (nps_radio_control_available(time)) {
+ radio_control_feed();
+ Fbw(event_task);
+ }
+#endif
+
+ if (nps_sensors_gyro_available()) {
+ imu_feed_gyro_accel();
+ Fbw(event_task);
+ Ap(event_task);
+ }
+
+ if (nps_sensors_mag_available()) {
+ imu_feed_mag();
+ Fbw(event_task);
+ Ap(event_task);
+ }
+
+ if (nps_sensors_baro_available()) {
+ /** @todo feed baro values */
+ //baro_feed_value(sensors.baro.value);
+ Fbw(event_task);
+ Ap(event_task);
+ }
+
+ if (nps_sensors_gps_available()) {
+ gps_feed_value();
+ Fbw(event_task);
+ Ap(event_task);
+ }
+
+ if (nps_bypass_ahrs) {
+ sim_overwrite_ahrs();
+ }
+
+ Fbw(handle_periodic_tasks);
+ Ap(handle_periodic_tasks);
+
+ /* scale final motor commands to 0-1 for feeding the fdm */
+ for (uint8_t i=0; i < COMMANDS_NB; i++)
+ autopilot.commands[i] = (double)commands[i]/MAX_PPRZ;
+
+}
+
+void sim_overwrite_ahrs(void) {
+
+ struct FloatQuat quat_f;
+ QUAT_COPY(quat_f, fdm.ltp_to_body_quat);
+ stateSetNedToBodyQuat_f(&quat_f);
+
+ struct FloatRates rates_f;
+ RATES_COPY(rates_f, fdm.body_ecef_rotvel);
+ stateSetBodyRates_f(&rates_f);
+
+}
diff --git a/sw/simulator/nps/nps_autopilot_fixedwing.h b/sw/simulator/nps/nps_autopilot_fixedwing.h
new file mode 100644
index 0000000000..a4d05a199b
--- /dev/null
+++ b/sw/simulator/nps/nps_autopilot_fixedwing.h
@@ -0,0 +1,10 @@
+#ifndef NPS_AUTOPILOT_FIXEDWING_H
+#define NPS_AUTOPILOT_FIXEDWING_H
+
+#include "nps_autopilot.h"
+
+
+extern bool_t nps_bypass_ahrs;
+extern void sim_overwrite_ahrs(void);
+
+#endif /* NPS_AUTOPILOT_FIXEDWING_H */
diff --git a/sw/simulator/nps/nps_autopilot_rotorcraft.c b/sw/simulator/nps/nps_autopilot_rotorcraft.c
index f87e000747..6641cc5dc8 100644
--- a/sw/simulator/nps/nps_autopilot_rotorcraft.c
+++ b/sw/simulator/nps/nps_autopilot_rotorcraft.c
@@ -24,38 +24,45 @@
#include "firmwares/rotorcraft/main.h"
#include "nps_sensors.h"
#include "nps_radio_control.h"
+#include "nps_electrical.h"
+#include "nps_fdm.h"
+
#include "subsystems/radio_control.h"
#include "subsystems/imu.h"
#include "subsystems/sensors/baro.h"
#include "baro_board.h"
-#include "subsystems/electrical.h"
#include "mcu_periph/sys_time.h"
#include "state.h"
+#include "subsystems/ahrs.h"
+#include "subsystems/ins.h"
+#include "math/pprz_algebra.h"
#include "subsystems/actuators/motor_mixing.h"
struct NpsAutopilot autopilot;
bool_t nps_bypass_ahrs;
+bool_t nps_bypass_ins;
#ifndef NPS_BYPASS_AHRS
#define NPS_BYPASS_AHRS FALSE
#endif
+#ifndef NPS_BYPASS_INS
+#define NPS_BYPASS_INS FALSE
+#endif
+
void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, char* rc_dev) {
nps_radio_control_init(type_rc, num_rc_script, rc_dev);
+ nps_electrical_init();
+
nps_bypass_ahrs = NPS_BYPASS_AHRS;
+ nps_bypass_ins = NPS_BYPASS_INS;
main_init();
-#ifdef MAX_BAT_LEVEL
- electrical.vsupply = MAX_BAT_LEVEL * 10;
-#else
- electrical.vsupply = 111;
-#endif
-
}
void nps_autopilot_run_systime_step( void ) {
@@ -65,7 +72,9 @@ void nps_autopilot_run_systime_step( void ) {
#include
#include "subsystems/gps.h"
-void nps_autopilot_run_step(double time __attribute__ ((unused))) {
+void nps_autopilot_run_step(double time) {
+
+ nps_electrical_run_step(time);
#ifdef RADIO_CONTROL_TYPE_PPM
if (nps_radio_control_available(time)) {
@@ -98,6 +107,10 @@ void nps_autopilot_run_step(double time __attribute__ ((unused))) {
sim_overwrite_ahrs();
}
+ if (nps_bypass_ins) {
+ sim_overwrite_ins();
+ }
+
handle_periodic_tasks();
/* scale final motor commands to 0-1 for feeding the fdm */
@@ -107,17 +120,31 @@ void nps_autopilot_run_step(double time __attribute__ ((unused))) {
}
-#include "nps_fdm.h"
-#include "subsystems/ahrs.h"
-#include "math/pprz_algebra.h"
+
void sim_overwrite_ahrs(void) {
- struct Int32Quat quat;
- QUAT_BFP_OF_REAL(quat, fdm.ltp_to_body_quat);
- stateSetNedToBodyQuat_i(&quat);
+ struct FloatQuat quat_f;
+ QUAT_COPY(quat_f, fdm.ltp_to_body_quat);
+ stateSetNedToBodyQuat_f(&quat_f);
- struct Int32Rates rates;
- RATES_BFP_OF_REAL(rates, fdm.body_ecef_rotvel);
- stateSetBodyRates_i(&rates);
+ struct FloatRates rates_f;
+ RATES_COPY(rates_f, fdm.body_ecef_rotvel);
+ stateSetBodyRates_f(&rates_f);
+
+}
+
+void sim_overwrite_ins(void) {
+
+ struct NedCoor_f ltp_pos;
+ VECT3_COPY(ltp_pos, fdm.ltpprz_pos);
+ stateSetPositionNed_f(<p_pos);
+
+ struct NedCoor_f ltp_speed;
+ VECT3_COPY(ltp_speed, fdm.ltpprz_ecef_vel);
+ stateSetSpeedNed_f(<p_speed);
+
+ struct NedCoor_f ltp_accel;
+ VECT3_COPY(ltp_accel, fdm.ltpprz_ecef_accel);
+ stateSetAccelNed_f(<p_accel);
}
diff --git a/sw/simulator/nps/nps_autopilot_rotorcraft.h b/sw/simulator/nps/nps_autopilot_rotorcraft.h
index 9d04b7213c..b71ffd7eb8 100644
--- a/sw/simulator/nps/nps_autopilot_rotorcraft.h
+++ b/sw/simulator/nps/nps_autopilot_rotorcraft.h
@@ -5,6 +5,8 @@
extern bool_t nps_bypass_ahrs;
+extern bool_t nps_bypass_ins;
extern void sim_overwrite_ahrs(void);
+extern void sim_overwrite_ins(void);
#endif /* NPS_AUTOPILOT_ROTORCRAFT_H */
diff --git a/sw/simulator/nps/nps_electrical.c b/sw/simulator/nps/nps_electrical.c
new file mode 100644
index 0000000000..97e66eb908
--- /dev/null
+++ b/sw/simulator/nps/nps_electrical.c
@@ -0,0 +1,46 @@
+/*
+ * Copyright (C) 2013 Felix Ruess
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file nps_electrical.c
+ * Electrical status (bat voltage) for NPS.
+ */
+
+#include "nps_electrical.h"
+#include "generated/airframe.h"
+#include "subsystems/electrical.h"
+
+struct NpsElectrical nps_electrical;
+
+void nps_electrical_init(void) {
+
+#ifdef MAX_BAT_LEVEL
+ nps_electrical.supply_voltage = MAX_BAT_LEVEL;
+#else
+ nps_electrical.supply_voltage = 11.1;
+#endif
+
+}
+
+void nps_electrical_run_step(double time __attribute__ ((unused))) {
+ // todo: auto-decrease bat voltage
+ electrical.vsupply = nps_electrical.supply_voltage * 10;
+}
diff --git a/sw/simulator/nps/nps_electrical.h b/sw/simulator/nps/nps_electrical.h
new file mode 100644
index 0000000000..d1b15bcfdb
--- /dev/null
+++ b/sw/simulator/nps/nps_electrical.h
@@ -0,0 +1,39 @@
+/*
+ * Copyright (C) 2013 Felix Ruess
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file nps_electrical.h
+ * Electrical status (bat voltage) for NPS.
+ */
+
+#ifndef NPS_ELECTRICAL_H
+#define NPS_ELECTRICAL_H
+
+struct NpsElectrical {
+ float supply_voltage;
+};
+
+extern struct NpsElectrical nps_electrical;
+
+extern void nps_electrical_init(void);
+extern void nps_electrical_run_step(double time);
+
+#endif
diff --git a/sw/simulator/nps/nps_fdm.h b/sw/simulator/nps/nps_fdm.h
index 022364c0ce..99c0ec8691 100644
--- a/sw/simulator/nps/nps_fdm.h
+++ b/sw/simulator/nps/nps_fdm.h
@@ -43,6 +43,7 @@ struct NpsFdm {
double init_dt;
double curr_dt;
bool_t on_ground;
+ int nan_count;
/* position */
struct EcefCoor_d ecef_pos;
@@ -85,11 +86,14 @@ struct NpsFdm {
struct DoubleVect3 ltp_g;
struct DoubleVect3 ltp_h;
+ struct DoubleVect3 wind; ///< velocity in m/s in NED
+
};
extern struct NpsFdm fdm;
extern void nps_fdm_init(double dt);
extern void nps_fdm_run_step(double* commands);
+extern void nps_fdm_set_wind(double speed, double dir, int turbulence_severity);
#endif /* NPS_FDM */
diff --git a/sw/simulator/nps/nps_fdm_jsbsim.c b/sw/simulator/nps/nps_fdm_jsbsim.c
index c2e5ebb95d..30a438d7e2 100644
--- a/sw/simulator/nps/nps_fdm_jsbsim.c
+++ b/sw/simulator/nps/nps_fdm_jsbsim.c
@@ -26,22 +26,42 @@
* This is an FDM for NPS that uses JSBSim as the simulation engine.
*/
+#include
+#include
+#include
+
#include
#include
#include
#include
#include
-#include
+#include
+
#include "nps_fdm.h"
-#include "generated/airframe.h"
#include "math/pprz_geodetic.h"
#include "math/pprz_geodetic_double.h"
#include "math/pprz_geodetic_float.h"
#include "math/pprz_algebra.h"
#include "math/pprz_algebra_float.h"
+#include "generated/airframe.h"
+#include "generated/flight_plan.h"
+
/// Macro to convert from feet to metres
#define MetersOfFeet(_f) ((_f)/3.2808399)
+#define FeetOfMeters(_m) ((_m)*3.2808399)
+
+/** Name of the JSBSim model.
+ * Defaults to the AIRFRAME_NAME
+ */
+#ifndef NPS_JSBSIM_MODEL
+#define NPS_JSBSIM_MODEL AIRFRAME_NAME
+#endif
+
+#ifdef NPS_INITIAL_CONDITITONS
+#warning NPS_INITIAL_CONDITITONS was replaced by NPS_JSBSIM_INIT!
+#warning Defaulting to flight plan location.
+#endif
/** Minimum JSBSim timestep
* Around 1/10000 seems to be good for ground impacts
@@ -49,9 +69,11 @@
#define MIN_DT (1.0/10240.0)
using namespace JSBSim;
+using namespace std;
static void feed_jsbsim(double* commands);
static void fetch_state(void);
+static int check_for_nan(void);
static void jsbsimvec_to_vec(DoubleVect3* fdm_vector, const FGColumnVector3* jsb_vector);
static void jsbsimloc_to_loc(EcefCoor_d* fdm_location, const FGLocation* jsb_location);
@@ -60,7 +82,6 @@ static void jsbsimvec_to_rate(DoubleRates* fdm_rate, const FGColumnVector3* jsb_
static void llh_from_jsbsim(LlaCoor_d* fdm_lla, FGPropagate* propagate);
static void lla_from_jsbsim_geodetic(LlaCoor_d* fdm_lla, FGPropagate* propagate);
static void lla_from_jsbsim_geocentric(LlaCoor_d* fdm_lla, FGPropagate* propagate);
-//static void rate_to_vec(DoubleVect3* vector, DoubleRates* rate);
static void init_jsbsim(double dt);
static void init_ltp(void);
@@ -87,6 +108,8 @@ void nps_fdm_init(double dt) {
for (min_dt = (1.0/dt); min_dt < (1/MIN_DT); min_dt += (1/dt)){}
min_dt = (1/min_dt);
+ fdm.nan_count = 0;
+
init_jsbsim(dt);
FDMExec->RunIC();
@@ -147,6 +170,24 @@ void nps_fdm_run_step(double* commands) {
fetch_state();
+ /* Check the current state to make sure it is valid (no NaNs) */
+ if (check_for_nan()) {
+ printf("Error: FDM simulation encountered a total of %i NaN values at simulation time %f.\n", fdm.nan_count, fdm.time);
+ printf("It is likely the simulation diverged and gave non-physical results. If you did\n");
+ printf("not crash, check your model and/or initial conditions. Exiting with status 1.\n");
+ exit(1);
+ }
+
+}
+
+void nps_fdm_set_wind(double speed, double dir, int turbulence_severity) {
+ FGWinds* Winds = FDMExec->GetWinds();
+ Winds->SetWindspeed(FeetOfMeters(speed));
+ Winds->SetWindPsi(dir);
+
+ /* wind speed used for turbulence */
+ Winds->SetWindspeed20ft(FeetOfMeters(speed)/2);
+ Winds->SetProbabilityOfExceedence(turbulence_severity);
}
/**
@@ -169,6 +210,7 @@ static void feed_jsbsim(double* commands) {
}
+
/**
* Populates the NPS fdm struct after a simulation step.
*/
@@ -265,9 +307,15 @@ static void fetch_state(void) {
/*
* rotational speed and accelerations
*/
- jsbsimvec_to_rate(&fdm.body_ecef_rotvel,&propagate->GetPQR());
- jsbsimvec_to_rate(&fdm.body_ecef_rotaccel,&accelerations->GetPQRdot());
+ jsbsimvec_to_rate(&fdm.body_ecef_rotvel, &propagate->GetPQR());
+ jsbsimvec_to_rate(&fdm.body_ecef_rotaccel, &accelerations->GetPQRdot());
+
+ /*
+ * wind
+ */
+ const FGColumnVector3& fg_wind_ned = FDMExec->GetWinds()->GetTotalWindNED();
+ jsbsimvec_to_vec(&fdm.wind, &fg_wind_ned);
}
/**
@@ -284,9 +332,18 @@ static void init_jsbsim(double dt) {
char buf[1024];
string rootdir;
+ string jsbsim_ic_name;
sprintf(buf,"%s/conf/simulator/jsbsim/",getenv("PAPARAZZI_HOME"));
rootdir = string(buf);
+
+ /* if jsbsim initial conditions are defined, use them
+ * otherwise use flightplan location
+ */
+#ifdef NPS_JSBSIM_INIT
+ jsbsim_ic_name = NPS_JSBSIM_INIT;
+#endif
+
FDMExec = new FGFDMExec();
FDMExec->Setsim_time(0.);
@@ -298,7 +355,7 @@ static void init_jsbsim(double dt) {
if ( ! FDMExec->LoadModel( rootdir + "aircraft",
rootdir + "engine",
rootdir + "systems",
- AIRFRAME_NAME,
+ NPS_JSBSIM_MODEL,
false)){
#ifdef DEBUG
cerr << " JSBSim could not be started" << endl << endl;
@@ -311,12 +368,39 @@ static void init_jsbsim(double dt) {
FDMExec->GetPropulsion()->InitRunning(-1);
JSBSim::FGInitialCondition *IC = FDMExec->GetIC();
- if ( ! IC->Load(NPS_INITIAL_CONDITITONS)) {
+ if(!jsbsim_ic_name.empty()) {
+ if ( ! IC->Load(jsbsim_ic_name)) {
#ifdef DEBUG
- cerr << "Initialization unsuccessful" << endl;
+ cerr << "Initialization unsuccessful" << endl;
#endif
- delete FDMExec;
- exit(-1);
+ delete FDMExec;
+ exit(-1);
+ }
+ }
+ else {
+ // FGInitialCondition::SetAltitudeASLFtIC
+ // requires this function to be called
+ // before itself
+ IC->SetVgroundFpsIC(0.);
+
+ // Use flight plan initial conditions
+ // convert geodetic lat from flight plan to geocentric
+ double gd_lat = RadOfDeg(NAV_LAT0 / 1e7);
+ double gc_lat = gc_of_gd_lat_d(gd_lat, GROUND_ALT);
+ IC->SetLatitudeDegIC(DegOfRad(gc_lat));
+ IC->SetLongitudeDegIC(NAV_LON0 / 1e7);
+
+ IC->SetAltitudeASLFtIC(FeetOfMeters(GROUND_ALT + 2.0));
+ IC->SetTerrainElevationFtIC(FeetOfMeters(GROUND_ALT));
+ IC->SetPsiDegIC(QFU);
+ IC->SetVgroundFpsIC(0.);
+
+ //initRunning for all engines
+ FDMExec->GetPropulsion()->InitRunning(-1);
+ if (!FDMExec->RunIC()) {
+ cerr << "Initialization from flight plan unsuccessful" << endl;
+ exit(-1);
+ }
}
// calculate vehicle max radius in m
@@ -473,13 +557,87 @@ void lla_from_jsbsim_geodetic(LlaCoor_d* fdm_lla, FGPropagate* propagate) {
}
-
-#if 0
-static void rate_to_vec(DoubleVect3* vector, DoubleRates* rate) {
-
- vector->x = rate->p;
- vector->y = rate->q;
- vector->z = rate->r;
-
-}
+#ifdef __APPLE__
+/* Why isn't this there when we include math.h (on osx with clang)? */
+/// Check if a double is NaN.
+static int isnan(double f) { return (f != f); }
#endif
+
+/**
+ * Checks NpsFdm struct for NaNs.
+ *
+ * Increments the NaN count on each new NaN
+ *
+ * @return Count of new NaNs. 0 for no new NaNs.
+ */
+static int check_for_nan(void) {
+ int orig_nan_count = fdm.nan_count;
+ /* Check all elements for nans */
+ if (isnan(fdm.ecef_pos.x)) fdm.nan_count++;
+ if (isnan(fdm.ecef_pos.y)) fdm.nan_count++;
+ if (isnan(fdm.ecef_pos.z)) fdm.nan_count++;
+ if (isnan(fdm.ltpprz_pos.x)) fdm.nan_count++;
+ if (isnan(fdm.ltpprz_pos.y)) fdm.nan_count++;
+ if (isnan(fdm.ltpprz_pos.z)) fdm.nan_count++;
+ if (isnan(fdm.lla_pos.lon)) fdm.nan_count++;
+ if (isnan(fdm.lla_pos.lat)) fdm.nan_count++;
+ if (isnan(fdm.lla_pos.alt)) fdm.nan_count++;
+ if (isnan(fdm.hmsl)) fdm.nan_count++;
+ // Skip debugging elements
+ if (isnan(fdm.ecef_ecef_vel.x)) fdm.nan_count++;
+ if (isnan(fdm.ecef_ecef_vel.y)) fdm.nan_count++;
+ if (isnan(fdm.ecef_ecef_vel.z)) fdm.nan_count++;
+ if (isnan(fdm.ecef_ecef_accel.x)) fdm.nan_count++;
+ if (isnan(fdm.ecef_ecef_accel.y)) fdm.nan_count++;
+ if (isnan(fdm.ecef_ecef_accel.z)) fdm.nan_count++;
+ if (isnan(fdm.body_ecef_vel.x)) fdm.nan_count++;
+ if (isnan(fdm.body_ecef_vel.y)) fdm.nan_count++;
+ if (isnan(fdm.body_ecef_vel.z)) fdm.nan_count++;
+ if (isnan(fdm.body_ecef_accel.x)) fdm.nan_count++;
+ if (isnan(fdm.body_ecef_accel.y)) fdm.nan_count++;
+ if (isnan(fdm.body_ecef_accel.z)) fdm.nan_count++;
+ if (isnan(fdm.ltp_ecef_vel.x)) fdm.nan_count++;
+ if (isnan(fdm.ltp_ecef_vel.y)) fdm.nan_count++;
+ if (isnan(fdm.ltp_ecef_vel.z)) fdm.nan_count++;
+ if (isnan(fdm.ltp_ecef_accel.x)) fdm.nan_count++;
+ if (isnan(fdm.ltp_ecef_accel.y)) fdm.nan_count++;
+ if (isnan(fdm.ltp_ecef_accel.z)) fdm.nan_count++;
+ if (isnan(fdm.ltpprz_ecef_vel.x)) fdm.nan_count++;
+ if (isnan(fdm.ltpprz_ecef_vel.y)) fdm.nan_count++;
+ if (isnan(fdm.ltpprz_ecef_vel.z)) fdm.nan_count++;
+ if (isnan(fdm.ltpprz_ecef_accel.x)) fdm.nan_count++;
+ if (isnan(fdm.ltpprz_ecef_accel.y)) fdm.nan_count++;
+ if (isnan(fdm.ltpprz_ecef_accel.z)) fdm.nan_count++;
+ if (isnan(fdm.ecef_to_body_quat.qi)) fdm.nan_count++;
+ if (isnan(fdm.ecef_to_body_quat.qx)) fdm.nan_count++;
+ if (isnan(fdm.ecef_to_body_quat.qy)) fdm.nan_count++;
+ if (isnan(fdm.ecef_to_body_quat.qz)) fdm.nan_count++;
+ if (isnan(fdm.ltp_to_body_quat.qi)) fdm.nan_count++;
+ if (isnan(fdm.ltp_to_body_quat.qx)) fdm.nan_count++;
+ if (isnan(fdm.ltp_to_body_quat.qy)) fdm.nan_count++;
+ if (isnan(fdm.ltp_to_body_quat.qz)) fdm.nan_count++;
+ if (isnan(fdm.ltp_to_body_eulers.phi)) fdm.nan_count++;
+ if (isnan(fdm.ltp_to_body_eulers.theta)) fdm.nan_count++;
+ if (isnan(fdm.ltp_to_body_eulers.psi)) fdm.nan_count++;
+ if (isnan(fdm.ltpprz_to_body_quat.qi)) fdm.nan_count++;
+ if (isnan(fdm.ltpprz_to_body_quat.qx)) fdm.nan_count++;
+ if (isnan(fdm.ltpprz_to_body_quat.qy)) fdm.nan_count++;
+ if (isnan(fdm.ltpprz_to_body_quat.qz)) fdm.nan_count++;
+ if (isnan(fdm.ltpprz_to_body_eulers.phi)) fdm.nan_count++;
+ if (isnan(fdm.ltpprz_to_body_eulers.theta)) fdm.nan_count++;
+ if (isnan(fdm.ltpprz_to_body_eulers.psi)) fdm.nan_count++;
+ if (isnan(fdm.body_ecef_rotvel.p)) fdm.nan_count++;
+ if (isnan(fdm.body_ecef_rotvel.q)) fdm.nan_count++;
+ if (isnan(fdm.body_ecef_rotvel.r)) fdm.nan_count++;
+ if (isnan(fdm.body_ecef_rotaccel.p)) fdm.nan_count++;
+ if (isnan(fdm.body_ecef_rotaccel.q)) fdm.nan_count++;
+ if (isnan(fdm.body_ecef_rotaccel.r)) fdm.nan_count++;
+ if (isnan(fdm.ltp_g.x)) fdm.nan_count++;
+ if (isnan(fdm.ltp_g.y)) fdm.nan_count++;
+ if (isnan(fdm.ltp_g.z)) fdm.nan_count++;
+ if (isnan(fdm.ltp_h.x)) fdm.nan_count++;
+ if (isnan(fdm.ltp_h.y)) fdm.nan_count++;
+ if (isnan(fdm.ltp_h.z)) fdm.nan_count++;
+
+ return (fdm.nan_count - orig_nan_count);
+}
diff --git a/sw/simulator/nps/nps_ivy.h b/sw/simulator/nps/nps_ivy.h
index 64011b7a1f..db8b993365 100644
--- a/sw/simulator/nps/nps_ivy.h
+++ b/sw/simulator/nps/nps_ivy.h
@@ -1,6 +1,7 @@
#ifndef NPS_IVY
#define NPS_IVY
+extern void nps_ivy_common_init(char* ivy_bus);
extern void nps_ivy_init(char* ivy_bus);
extern void nps_ivy_display(void);
diff --git a/sw/simulator/nps/nps_ivy.c b/sw/simulator/nps/nps_ivy_common.c
similarity index 85%
rename from sw/simulator/nps/nps_ivy.c
rename to sw/simulator/nps/nps_ivy_common.c
index 93847c425c..4c869a22ac 100644
--- a/sw/simulator/nps/nps_ivy.c
+++ b/sw/simulator/nps/nps_ivy_common.c
@@ -9,8 +9,9 @@
#include "nps_autopilot.h"
#include "nps_fdm.h"
#include "nps_sensors.h"
+#include "nps_atmosphere.h"
#include "subsystems/ins.h"
-#include "firmwares/rotorcraft/navigation.h"
+#include "subsystems/navigation/common_flight_plan.h"
#ifdef RADIO_CONTROL_TYPE_DATALINK
#include "subsystems/radio_control.h"
@@ -36,10 +37,6 @@ static void on_DL_BLOCK(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]);
-static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
- void *user_data __attribute__ ((unused)),
- int argc __attribute__ ((unused)), char *argv[]);
-
#ifdef RADIO_CONTROL_TYPE_DATALINK
static void on_DL_RC_3CH(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
@@ -50,7 +47,7 @@ static void on_DL_RC_4CH(IvyClientPtr app __attribute__ ((unused)),
int argc __attribute__ ((unused)), char *argv[]);
#endif
-void nps_ivy_init(char* ivy_bus) {
+void nps_ivy_common_init(char* ivy_bus) {
const char* agent_name = AIRFRAME_NAME"_NPS";
const char* ready_msg = AIRFRAME_NAME"_NPS Ready";
IvyInit(agent_name, ready_msg, NULL, NULL, NULL, NULL);
@@ -58,7 +55,6 @@ void nps_ivy_init(char* ivy_bus) {
IvyBindMsg(on_DL_SETTING, NULL, "^(\\S*) DL_SETTING (\\S*) (\\S*) (\\S*)");
IvyBindMsg(on_DL_GET_SETTING, NULL, "^(\\S*) GET_DL_SETTING (\\S*) (\\S*)");
IvyBindMsg(on_DL_BLOCK, NULL, "^(\\S*) BLOCK (\\S*) (\\S*)");
- IvyBindMsg(on_DL_MOVE_WP, NULL, "^(\\S*) MOVE_WP (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
#ifdef RADIO_CONTROL_TYPE_DATALINK
IvyBindMsg(on_DL_RC_3CH, NULL, "^(\\S*) RC_3CH (\\S*) (\\S*) (\\S*) (\\S*)");
@@ -117,25 +113,6 @@ static void on_DL_BLOCK(IvyClientPtr app __attribute__ ((unused)),
printf("goto block %d\n", block);
}
-static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
- void *user_data __attribute__ ((unused)),
- int argc __attribute__ ((unused)), char *argv[]) {
- uint8_t wp_id = atoi(argv[1]);
-
- struct LlaCoor_i lla;
- struct EnuCoor_i enu;
- lla.lat = INT32_RAD_OF_DEG(atoi(argv[3]));
- lla.lon = INT32_RAD_OF_DEG(atoi(argv[4]));
- lla.alt = atoi(argv[5])*10 - ins_ltp_def.hmsl + ins_ltp_def.lla.alt;
- enu_of_lla_point_i(&enu,&ins_ltp_def,&lla);
- enu.x = POS_BFP_OF_REAL(enu.x)/100;
- enu.y = POS_BFP_OF_REAL(enu.y)/100;
- enu.z = POS_BFP_OF_REAL(enu.z)/100;
- VECT3_ASSIGN(waypoints[wp_id], enu.x, enu.y, enu.z);
- DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &enu.x, &enu.y, &enu.z);
- printf("move wp id=%d x=%d y=%d z=%d\n", wp_id, enu.x, enu.y, enu.z);
-}
-
#ifdef RADIO_CONTROL_TYPE_DATALINK
static void on_DL_RC_3CH(IvyClientPtr app __attribute__ ((unused)),
void *user_data __attribute__ ((unused)),
@@ -210,4 +187,10 @@ void nps_ivy_display(void) {
h_body.x,
h_body.y,
h_body.z);
+
+ IvySendMsg("%d NPS_WIND %f %f %f",
+ AC_ID,
+ fdm.wind.x,
+ fdm.wind.y,
+ fdm.wind.z);
}
diff --git a/sw/simulator/nps/nps_ivy_fixedwing.c b/sw/simulator/nps/nps_ivy_fixedwing.c
new file mode 100644
index 0000000000..a7bd73cbba
--- /dev/null
+++ b/sw/simulator/nps/nps_ivy_fixedwing.c
@@ -0,0 +1,56 @@
+#include "nps_ivy.h"
+
+#include
+#include
+
+#include "generated/airframe.h"
+#include "math/pprz_algebra_double.h"
+#include "subsystems/ins.h"
+#include "subsystems/navigation/common_nav.h"
+
+/* fixedwing specific Datalink Ivy functions */
+void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
+ void *user_data __attribute__ ((unused)),
+ int argc __attribute__ ((unused)), char *argv[]);
+
+void nps_ivy_init(char* ivy_bus) {
+ /* init ivy and bind some messages common to fw and rotorcraft */
+ nps_ivy_common_init(ivy_bus);
+
+ IvyBindMsg(on_DL_MOVE_WP, NULL, "^(\\S*) MOVE_WP (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
+}
+
+//TODO use datalink parsing from fixedwing instead of doing it here explicitly
+
+#include "generated/settings.h"
+#include "dl_protocol.h"
+#include "subsystems/datalink/downlink.h"
+
+#define MOfCm(_x) (((float)(_x))/100.)
+
+void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
+ void *user_data __attribute__ ((unused)),
+ int argc __attribute__ ((unused)), char *argv[]) {
+
+ if (atoi(argv[2]) == AC_ID) {
+ uint8_t wp_id = atoi(argv[1]);
+ float a = MOfCm(atoi(argv[5]));
+
+ /* Computes from (lat, long) in the referenced UTM zone */
+ struct LlaCoor_f lla;
+ lla.lat = RadOfDeg((float)(atoi(argv[3]) / 1e7));
+ lla.lon = RadOfDeg((float)(atoi(argv[4]) / 1e7));
+ //printf("move wp id=%d lat=%f lon=%f alt=%f\n", wp_id, lla.lat, lla.lon, a);
+ struct UtmCoor_f utm;
+ utm.zone = nav_utm_zone0;
+ utm_of_lla_f(&utm, &lla);
+ nav_move_waypoint(wp_id, utm.east, utm.north, a);
+
+ /* Waypoint range is limited. Computes the UTM pos back from the relative
+ coordinates */
+ utm.east = waypoints[wp_id].x + nav_utm_east0;
+ utm.north = waypoints[wp_id].y + nav_utm_north0;
+ DOWNLINK_SEND_WP_MOVED(DefaultChannel, DefaultDevice, &wp_id, &utm.east, &utm.north, &a, &nav_utm_zone0);
+ printf("move wp id=%d east=%f north=%f zone=%i alt=%f\n", wp_id, utm.east, utm.north, utm.zone, a);
+ }
+}
diff --git a/sw/simulator/nps/nps_ivy_rotorcraft.c b/sw/simulator/nps/nps_ivy_rotorcraft.c
new file mode 100644
index 0000000000..bf0ef5562b
--- /dev/null
+++ b/sw/simulator/nps/nps_ivy_rotorcraft.c
@@ -0,0 +1,58 @@
+#include "nps_ivy.h"
+
+#include
+#include
+
+#include "generated/airframe.h"
+#include "math/pprz_algebra_double.h"
+#include "nps_autopilot.h"
+#include "nps_fdm.h"
+#include "nps_sensors.h"
+#include "subsystems/ins.h"
+#include "firmwares/rotorcraft/navigation.h"
+
+#ifdef RADIO_CONTROL_TYPE_DATALINK
+#include "subsystems/radio_control.h"
+#endif
+
+#include NPS_SENSORS_PARAMS
+
+
+/* rotorcraft specificDatalink Ivy functions */
+static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
+ void *user_data __attribute__ ((unused)),
+ int argc __attribute__ ((unused)), char *argv[]);
+
+void nps_ivy_init(char* ivy_bus) {
+ /* init ivy and bind some messages common to fw and rotorcraft */
+ nps_ivy_common_init(ivy_bus);
+
+ IvyBindMsg(on_DL_MOVE_WP, NULL, "^(\\S*) MOVE_WP (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
+
+}
+
+//TODO use datalink parsing from rotorcraft instead of doing it here explicitly
+
+#include "generated/settings.h"
+#include "dl_protocol.h"
+#include "subsystems/datalink/downlink.h"
+static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
+ void *user_data __attribute__ ((unused)),
+ int argc __attribute__ ((unused)), char *argv[]) {
+ if (atoi(argv[2]) == AC_ID) {
+ uint8_t wp_id = atoi(argv[1]);
+
+ struct LlaCoor_i lla;
+ struct EnuCoor_i enu;
+ lla.lat = INT32_RAD_OF_DEG(atoi(argv[3]));
+ lla.lon = INT32_RAD_OF_DEG(atoi(argv[4]));
+ lla.alt = atoi(argv[5])*10 - ins_ltp_def.hmsl + ins_ltp_def.lla.alt;
+ enu_of_lla_point_i(&enu,&ins_ltp_def,&lla);
+ enu.x = POS_BFP_OF_REAL(enu.x)/100;
+ enu.y = POS_BFP_OF_REAL(enu.y)/100;
+ enu.z = POS_BFP_OF_REAL(enu.z)/100;
+ VECT3_ASSIGN(waypoints[wp_id], enu.x, enu.y, enu.z);
+ DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &enu.x, &enu.y, &enu.z);
+ printf("move wp id=%d x=%d y=%d z=%d\n", wp_id, enu.x, enu.y, enu.z);
+ }
+}
diff --git a/sw/simulator/nps/nps_main.c b/sw/simulator/nps/nps_main.c
index a740eb15e0..6686beb324 100644
--- a/sw/simulator/nps/nps_main.c
+++ b/sw/simulator/nps/nps_main.c
@@ -84,7 +84,7 @@ double time_to_double(struct timeval *t) {
return ((double)t->tv_sec + (double)(t->tv_usec * 1e-6));
}
-int main ( int argc, char** argv) {
+int main (int argc, char** argv) {
if (!nps_main_parse_options(argc, argv)) return 1;
@@ -121,6 +121,7 @@ static void nps_main_init(void) {
nps_ivy_init(nps_main.ivy_bus);
nps_fdm_init(SIM_DT);
+ nps_atmosphere_init();
nps_sensors_init(nps_main.sim_time);
printf("Simulating with dt of %f\n", SIM_DT);
@@ -153,6 +154,8 @@ static void nps_main_init(void) {
static void nps_main_run_sim_step(void) {
// printf("sim at %f\n", nps_main.sim_time);
+ nps_atmosphere_update(SIM_DT);
+
nps_autopilot_run_systime_step();
nps_fdm_run_step(autopilot.commands);
diff --git a/sw/simulator/nps/nps_radio_control_joystick.c b/sw/simulator/nps/nps_radio_control_joystick.c
index 746c04aed4..4b164777b2 100644
--- a/sw/simulator/nps/nps_radio_control_joystick.c
+++ b/sw/simulator/nps/nps_radio_control_joystick.c
@@ -27,13 +27,49 @@
* Simple DirectMedia Layer library is used for cross-platform support.
* Joystick button and axes are mapped to RC commands directly with defines.
*
- * Currently it doesn't support different RC configurations.
+ * You must have a joystick with either:
+ * - 4 axes and 3 buttons, or
+ * - 5 axes
+ *
+ * First you should run sw/ground_segment/joystick/test_stick to determine
+ * the indices of the physical axes and buttons on your joystick (and to test
+ * that it actually works). Then you can assign each axis and button to a
+ * command in your airframe file.
+ *
+ * The default axes are Roll = 0, Pitch = 1, Yaw = 2 and Throttle = 3.
+ * The default buttons are Manual = 0, Auto1 = 1, Auto2 = 2.
+ *
+ * Example for 4 axes and 3 buttons using a joystick with 7 axes and 5 buttons:
+ * @verbatim
+ *
+ * ...
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ * @endverbatim
+ *
+ * One can define NPS_JS_AXIS_MODE to use an axis instead of buttons to change
+ * You will need a 5-axis joystick.
+ *
+ * If you need to reverse the direction of any axis, simply use:
+ * @verbatim
+ *
+ *
+ * @endverbatim
+ *
+ * At this point, no other functionality or channels are supported for R/C control.
+ *
*/
#include "nps_radio_control.h"
#include "nps_radio_control_joystick.h"
-// for NPS_JS_MODE_AXIS
+// for NPS_JS_AXIS_MODE
#include "generated/airframe.h"
#include
@@ -42,22 +78,37 @@
#include
// axes indice
-#define JS_ROLL 0
-#define JS_PITCH 1
-#define JS_YAW 2
-#define JS_THROTTLE 3
+#ifndef NPS_JS_AXIS_ROLL
+#define NPS_JS_AXIS_ROLL 0
+#endif
+#ifndef NPS_JS_AXIS_PITCH
+#define NPS_JS_AXIS_PITCH 1
+#endif
+#ifndef NPS_JS_AXIS_YAW
+#define NPS_JS_AXIS_YAW 2
+#endif
+#ifndef NPS_JS_AXIS_THROTTLE
+#define NPS_JS_AXIS_THROTTLE 3
+#endif
-#ifndef NPS_JS_MODE_AXIS
+#ifndef NPS_JS_AXIS_MODE
#define JS_NB_AXIS 4
#else
#define JS_NB_AXIS 5
#endif
// buttons to switch modes
-#define JS_BUTTON_MODE_MANUAL 4
-#define JS_BUTTON_MODE_AUTO1 5
-#define JS_BUTTON_MODE_AUTO2 6
-#ifndef NPS_JS_MODE_AXIS
+#ifndef NPS_JS_BUTTON_MODE_MANUAL
+#define NPS_JS_BUTTON_MODE_MANUAL 1
+#endif
+#ifndef NPS_JS_BUTTON_MODE_AUTO1
+#define NPS_JS_BUTTON_MODE_AUTO1 2
+#endif
+#ifndef NPS_JS_BUTTON_MODE_AUTO2
+#define NPS_JS_BUTTON_MODE_AUTO2 3
+#endif
+
+#ifndef NPS_JS_AXIS_MODE
#define JS_NB_BUTTONS 3
#else
#define JS_NB_BUTTONS 0
@@ -132,6 +183,8 @@ int nps_radio_control_joystick_init(const char* device) {
else if (SDL_JoystickNumAxes(sdl_joystick) < JS_NB_AXIS)
{
printf("Selected joystick does not support enough axes!\n");
+ printf("Number of axes required: %i\n", JS_NB_AXIS);
+ printf("Number of axes available: %i\n",SDL_JoystickNumAxes(sdl_joystick));
SDL_JoystickClose(sdl_joystick);
exit(-1);
}
@@ -155,13 +208,33 @@ int nps_radio_control_joystick_init(const char* device) {
*/
void nps_radio_control_joystick_update(void) {
- nps_joystick.throttle = ((float)(SDL_JoystickGetAxis(sdl_joystick,JS_THROTTLE)) - 32767.)/-65534.;
- nps_joystick.roll = (float)(SDL_JoystickGetAxis(sdl_joystick,JS_ROLL))/32767.;
- nps_joystick.pitch = (float)(SDL_JoystickGetAxis(sdl_joystick,JS_PITCH))/32767.;
- nps_joystick.yaw = (float)(SDL_JoystickGetAxis(sdl_joystick,JS_YAW))/32767.;
+#if NPS_JS_AXIS_THROTTLE_REVERSED
+ nps_joystick.throttle = ((float)(-1 * SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_THROTTLE)) - 32767.)/-65534.;
+#else
+ nps_joystick.throttle = ((float)(SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_THROTTLE)) - 32767.)/-65534.;
+#endif
+#if NPS_JS_AXIS_ROLL_REVERSED
+ nps_joystick.roll = (float)(-1 * SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_ROLL))/32767.;
+#else
+ nps_joystick.roll = (float)(SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_ROLL))/32767.;
+#endif
+#if NPS_JS_AXIS_PITCH_REVERSED
+ nps_joystick.pitch = (float)(-1 * SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_PITCH))/32767.;
+#else
+ nps_joystick.pitch = (float)(SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_PITCH))/32767.;
+#endif
+#if NPS_JS_AXIS_YAW_REVERSED
+ nps_joystick.yaw = (float)(-1 * SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_YAW))/32767.;
+#else
+ nps_joystick.yaw = (float)(SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_YAW))/32767.;
+#endif
// if an axis is asigned to the mode, use it instead of the buttons
-#ifdef NPS_JS_MODE_AXIS
- nps_joystick.mode = (float)(SDL_JoystickGetAxis(sdl_joystick,NPS_JS_MODE_AXIS))/32767.;
+#ifdef NPS_JS_AXIS_MODE
+#if NPS_JS_AXIS_MODE_REVERSED
+ nps_joystick.mode = (float)(-1 * SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_MODE))/32767.;
+#else
+ nps_joystick.mode = (float)(SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_MODE))/32767.;
+#endif
#endif
while(SDL_PollEvent(&sdl_event))
@@ -172,16 +245,16 @@ void nps_radio_control_joystick_update(void) {
{
switch(sdl_event.jbutton.button)
{
-#ifndef NPS_JS_MODE_AXIS
- case JS_BUTTON_MODE_MANUAL:
+#ifndef NPS_JS_AXIS_MODE
+ case NPS_JS_BUTTON_MODE_MANUAL:
nps_joystick.mode = MODE_SWITCH_MANUAL;
break;
- case JS_BUTTON_MODE_AUTO1:
+ case NPS_JS_BUTTON_MODE_AUTO1:
nps_joystick.mode = MODE_SWITCH_AUTO1;
break;
- case JS_BUTTON_MODE_AUTO2:
+ case NPS_JS_BUTTON_MODE_AUTO2:
nps_joystick.mode = MODE_SWITCH_AUTO2;
break;
#endif
diff --git a/sw/simulator/pprzsim-launch b/sw/simulator/pprzsim-launch
index c126dc90bb..877c292948 100755
--- a/sw/simulator/pprzsim-launch
+++ b/sw/simulator/pprzsim-launch
@@ -56,6 +56,10 @@ def main():
help="Boot the aircraft on start")
ocamlsim_opts.add_option("--norc", dest="norc", action="store_true",
help="Hide the simulated RC")
+ ocamlsim_opts.add_option("--noground", dest="noground", action="store_true",
+ help="Disable ground detection")
+ ocamlsim_opts.add_option("--launch", dest="launch", action="store_true",
+ help="Launch the aircraft on startup")
# special options for NPS
nps_opts = OptionGroup(parser, "NPS Options", "These are only relevant to the NPS sim")
@@ -94,6 +98,10 @@ def main():
simargs.append("-boot")
if options.norc:
simargs.append("-norc")
+ if options.noground:
+ simargs.append("-noground")
+ if options.launch:
+ simargs.append("-launch")
if options.ivy_bus:
simargs.append("-b")
simargs.append(options.ivy_bus)
diff --git a/sw/supervision/paparazzicenter.glade b/sw/supervision/paparazzicenter.glade
index c59f210306..e68aa24f77 100644
--- a/sw/supervision/paparazzicenter.glade
+++ b/sw/supervision/paparazzicenter.glade
@@ -276,6 +276,16 @@
+
+
+
diff --git a/sw/supervision/paparazzicenter.ml b/sw/supervision/paparazzicenter.ml
index b3af7d514e..966475d7c6 100644
--- a/sw/supervision/paparazzicenter.ml
+++ b/sw/supervision/paparazzicenter.ml
@@ -173,7 +173,8 @@ let () =
if Sys.file_exists Utils.backup_xml_file then begin
let rec question_box = fun () ->
- match GToolbox.question_box ~title:"Backup" ~buttons:["Keep changes"; "Discard changes"; "View changes"] ~default:2 "Configuration changes made during the last session were not saved. ?" with
+ let message = "Configuration changes to conf/conf.xml were not saved during the last session.\nIf you made any manual changes to conf/conf.xml and choose [Discard changes] you will also lose these." in
+ match GToolbox.question_box ~title:"Backup" ~buttons:["Keep changes"; "Discard changes"; "View changes"] ~default:2 message with
| 2 -> Sys.rename Utils.backup_xml_file Utils.conf_xml_file
| 3 -> ignore (Sys.command (sprintf "meld %s %s" Utils.backup_xml_file Utils.conf_xml_file)); question_box ()
| _ -> Sys.remove Utils.backup_xml_file in
@@ -257,6 +258,11 @@ let () =
ignore (GToolbox.message_box ~title:"About Paparazzi Center" ~icon:(GMisc.image ~pixbuf:paparazzi_pixbuf ())#coerce "Copyright (C) 2007-2008 ENAC, Pascal Brisset\nhttp://paparazzi.enac.fr") in
ignore (gui#menu_item_about#connect#activate ~callback);
+ (* Help/Get Help menu entry *)
+ let callback = fun () ->
+ ignore (GToolbox.message_box ~title:"Getting Help with Paparazzi" ~icon:(GMisc.image ~pixbuf:paparazzi_pixbuf ())#coerce "The primary documentation for Paparazzi is on the wiki:\nhttp://paparazzi.enac.fr\n\nCommunity-based support is through the paparazzi-devel mailing list:\nhttp://paparazzi.enac.fr/wiki/Contact\n\nThe Paparazzi auto-generated developer documentation is found on GitHub:\nhttp://paparazzi.github.io/docs/\n\nThe Paparazzi sourcecode can be found on GitHub:\nhttps://github.com/paparazzi/paparazzi\n\nIf you think you have found a bug or would like to make a feature request, feel\nfree to visit the Issues page found on GitHub (link found on the above webpage).") in
+ ignore (gui#menu_item_get_help#connect#activate ~callback);
+
(* Read preferences *)
if Sys.file_exists Env.gconf_file then begin
read_preferences gui Env.gconf_file ac_combo session_combo target_combo
diff --git a/sw/supervision/pc_aircraft.ml b/sw/supervision/pc_aircraft.ml
index 1c95fd3b9d..1537747e69 100644
--- a/sw/supervision/pc_aircraft.ml
+++ b/sw/supervision/pc_aircraft.ml
@@ -79,7 +79,8 @@ let new_ac_id = fun () ->
let parse_conf_xml = fun vbox ->
let strings = ref [] in
Hashtbl.iter (fun name _ac -> strings := name :: !strings) Utils.aircrafts;
- Gtk_tools.combo ("" :: !strings) vbox
+ let ordered = List.sort String.compare ("" :: !strings) in
+ Gtk_tools.combo ordered vbox
let editor =
try Sys.getenv "EDITOR" with _ -> (
diff --git a/sw/supervision/pc_control_panel.ml b/sw/supervision/pc_control_panel.ml
index a476317842..2793706092 100644
--- a/sw/supervision/pc_control_panel.ml
+++ b/sw/supervision/pc_control_panel.ml
@@ -195,10 +195,11 @@ let supervision = fun ?file gui log (ac_combo : Gtk_tools.combo) (target_combo :
Gtk_tools.add_to_combo session_combo "Simulation";
Gtk_tools.add_to_combo session_combo "Replay";
Gtk_tools.add_to_combo session_combo Gtk_tools.combo_separator;
- Hashtbl.iter
- (fun name _session ->
- Gtk_tools.add_to_combo session_combo name)
- sessions in
+ let strings = ref [] in
+ Hashtbl.iter (fun name _session -> strings := name :: !strings) sessions;
+ let ordered = List.sort String.compare !strings in
+ List.iter (fun name -> Gtk_tools.add_to_combo session_combo name) ordered
+ in
register_custom_sessions ();
Gtk_tools.select_in_combo session_combo "Simulation";
diff --git a/sw/tools/calibration/calibrate.py b/sw/tools/calibration/calibrate.py
index 2fb739ff18..c67aa7df0b 100755
--- a/sw/tools/calibration/calibrate.py
+++ b/sw/tools/calibration/calibrate.py
@@ -20,6 +20,7 @@
# Boston, MA 02111-1307, USA.
#
+from __future__ import print_function
import sys
import os
@@ -42,6 +43,9 @@ def main():
parser.add_option("-p", "--plot",
help="Show resulting plots",
action="store_true", dest="plot")
+ parser.add_option("-a", "--auto_threshold",
+ help="Try to automatically determine noise threshold",
+ action="store_true", dest="auto_threshold")
parser.add_option("-v", "--verbose",
action="store_true", dest="verbose")
(options, args) = parser.parse_args()
@@ -86,12 +90,21 @@ def main():
if options.verbose:
print("found "+str(len(measurements))+" records")
+ # estimate the noise threshold
+ # find the median of measurement vector lenght
+ if options.auto_threshold:
+ meas_median = scipy.median(scipy.array([scipy.linalg.norm(v) for v in measurements]))
+ # set noise threshold to be below 10% of that
+ noise_threshold = meas_median * 0.1
+ if options.verbose:
+ print("Using noise threshold of", noise_threshold, "for filtering.")
+
# filter out noisy measurements
flt_meas, flt_idx = calibration_utils.filter_meas(measurements, noise_window, noise_threshold)
if options.verbose:
- print("remaining "+str(len(flt_meas))+" after low pass")
+ print("remaining "+str(len(flt_meas))+" after filtering")
if len(flt_meas) == 0:
- print("Error: found zero IMU_"+options.sensor+"_RAW measurements for aircraft with id "+options.ac_id+" in log file after low pass!")
+ print("Error: found zero IMU_"+options.sensor+"_RAW measurements for aircraft with id "+options.ac_id+" in log file after filtering!")
sys.exit(1)
# get an initial min/max guess
diff --git a/sw/tools/calibration/calibration_utils.py b/sw/tools/calibration/calibration_utils.py
index c079fd8bb9..0cc8f54b75 100644
--- a/sw/tools/calibration/calibration_utils.py
+++ b/sw/tools/calibration/calibration_utils.py
@@ -200,7 +200,6 @@ def plot_mag_3d(measured, calibrated, p):
mx = measured[:, 0]
my = measured[:, 1]
mz = measured[:, 2]
- m_max = amax(abs(measured))
# calibrated values
cx = calibrated[:, 0]
@@ -224,17 +223,24 @@ def plot_mag_3d(measured, calibrated, p):
ax.scatter(mx, my, mz)
hold(True)
# plot line from center to ellipsoid center
- ax.plot([0.0, p[0]], [0.0, p[1]], [0.0, p[2]], color='black', marker='+')
+ ax.plot([0.0, p[0]], [0.0, p[1]], [0.0, p[2]], color='black', marker='+', markersize=10)
# plot ellipsoid
ax.plot_wireframe(ex, ey, ez, color='grey', alpha=0.5)
+ # Create cubic bounding box to simulate equal aspect ratio
+ max_range = np.array([mx.max()-mx.min(), my.max()-my.min(), mz.max()-mz.min()]).max()
+ Xb = 0.5*max_range*np.mgrid[-1:2:2,-1:2:2,-1:2:2][0].flatten() + 0.5*(mx.max()+mx.min())
+ Yb = 0.5*max_range*np.mgrid[-1:2:2,-1:2:2,-1:2:2][1].flatten() + 0.5*(my.max()+my.min())
+ Zb = 0.5*max_range*np.mgrid[-1:2:2,-1:2:2,-1:2:2][2].flatten() + 0.5*(mz.max()+mz.min())
+ # uncomment following both lines to test the fake bounding box:
+ #for xb, yb, zb in zip(Xb, Yb, Zb):
+ # ax.plot([xb], [yb], [zb], 'r*')
+
ax.set_title('MAG raw with fitted ellipsoid and center offset')
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
- ax.set_xlim3d(-m_max, m_max)
- ax.set_ylim3d(-m_max, m_max)
- ax.set_zlim3d(-m_max, m_max)
+
if matplotlib.__version__.startswith('0'):
ax = Axes3D(fig, rect=rect_r)
diff --git a/sw/tools/dfu/stm32_mem.py b/sw/tools/dfu/stm32_mem.py
index d6da3e6831..7219e7efd3 100644
--- a/sw/tools/dfu/stm32_mem.py
+++ b/sw/tools/dfu/stm32_mem.py
@@ -76,6 +76,21 @@ def print_copyright():
print("License GPLv3+: GNU GPL version 3 or later ")
print("")
+def init_progress_bar():
+ max_symbols = 50
+ print("[0%" + "="*int(max_symbols/2 - 4) + "50%" + "="*int(max_symbols/2 - 4) + "100%]")
+ print(" ", end="")
+ update_progress_bar.count = 0
+ update_progress_bar.symbol_limit = max_symbols
+
+def update_progress_bar(completed, total):
+ if completed and total:
+ percent = 100 * (float(completed)/float(total))
+ if (percent >= (update_progress_bar.count + (100.0 / update_progress_bar.symbol_limit))):
+ update_progress_bar.count += (100.0 / update_progress_bar.symbol_limit)
+ print("#", end="")
+ stdout.flush()
+
if __name__ == "__main__":
usage = "Usage: %prog [options] firmware.bin" + "\n" + "Run %prog --help to list the options."
parser = OptionParser(usage, version='%prog version 1.3')
@@ -121,6 +136,7 @@ if __name__ == "__main__":
valid_manufacturers.append("Transition Robotics Inc.")
valid_manufacturers.append("STMicroelectronics")
valid_manufacturers.append("Black Sphere Technologies")
+ valid_manufacturers.append("TUDelft MavLab. 2012->13")
# list of tuples with possible stm32 (autopilot) devices
stm32devs = []
@@ -152,7 +168,7 @@ if __name__ == "__main__":
if options.product == "any":
stm32devs.append((dfudev, man, product, serial))
elif options.product == "Lisa/Lia":
- if "Lisa/M" in product or "Lia" in product:
+ if "Lisa/M" in product or "Lia" in product or "Fireswarm" in product:
stm32devs.append((dfudev, man, product, serial))
if not stm32devs:
@@ -193,20 +209,26 @@ if __name__ == "__main__":
print("Could not open binary file.")
raise
+ # Get the file length for progress bar
+ bin_length = len(bin)
+
#addr = APP_ADDRESS
addr = options.addr
print ("Programming memory from 0x%08X...\r" % addr)
+ init_progress_bar()
+
while bin:
-# print("Programming memory at 0x%08X\r" % addr),
- print('#', end="")
- stdout.flush()
+ update_progress_bar((addr - options.addr),bin_length)
stm32_erase(target, addr)
stm32_write(target, bin[:SECTOR_SIZE])
bin = bin[SECTOR_SIZE:]
addr += SECTOR_SIZE
+ # Need to check all the way to 100% complete
+ update_progress_bar((addr - options.addr),bin_length)
+
stm32_manifest(target)
print("\nAll operations complete!\n")
diff --git a/sw/tools/gen_aircraft.ml b/sw/tools/gen_aircraft.ml
index e82a21c4c9..3065443de0 100644
--- a/sw/tools/gen_aircraft.ml
+++ b/sw/tools/gen_aircraft.ml
@@ -264,12 +264,13 @@ let dump_firmware_sections = fun xml makefile_ac ->
(** Extracts the makefile sections of an airframe file *)
-let extract_makefile = fun airframe_file makefile_ac ->
+let extract_makefile = fun ac_id airframe_file makefile_ac ->
let xml = Xml.parse_file airframe_file in
let f = open_out makefile_ac in
fprintf f "# This file has been generated from %s by %s\n" airframe_file Sys.argv.(0);
fprintf f "# Please DO NOT EDIT\n";
+ fprintf f "AC_ID=%s\n" ac_id;
(** Search and dump makefile sections that have a "location" attribute set to "before" or no attribute *)
dump_makefile_section xml f airframe_file "before";
@@ -397,7 +398,7 @@ let () =
let temp_makefile_ac = Filename.temp_file "Makefile.ac" "tmp" in
let abs_airframe_file = paparazzi_conf // airframe_file in
- let modules_files = extract_makefile abs_airframe_file temp_makefile_ac in
+ let modules_files = extract_makefile (value "ac_id") abs_airframe_file temp_makefile_ac in
(* Create Makefile.ac only if needed *)
let makefile_ac = aircraft_dir // "Makefile.ac" in
diff --git a/sw/tools/gen_airframe.ml b/sw/tools/gen_airframe.ml
index a8d0c34221..d18fead422 100644
--- a/sw/tools/gen_airframe.ml
+++ b/sw/tools/gen_airframe.ml
@@ -229,7 +229,7 @@ let parse_command = fun command no ->
let failsafe_value = int_of_string (ExtXml.attrib command "failsafe_value") in
{ failsafe_value = failsafe_value; foo = 0}
-let rec parse_section = fun s ->
+let rec parse_section = fun ac_id s ->
match Xml.tag s with
"section" ->
let prefix = ExtXml.attrib_or_default s "prefix" "" in
@@ -281,11 +281,11 @@ let rec parse_section = fun s ->
List.iter (fun d -> printf " Actuators%sInit();\\\n" d) drivers;
printf "}\n\n";
| "include" ->
- let filename = ExtXml.attrib s "href" in
+ let filename = Str.global_replace (Str.regexp "\\$AC_ID") ac_id (ExtXml.attrib s "href") in
let subxml = Xml.parse_file filename in
printf "/* XML %s */" filename;
nl ();
- List.iter parse_section (Xml.children subxml)
+ List.iter (parse_section ac_id) (Xml.children subxml)
| "makefile" ->
()
(** Ignoring this section *)
@@ -320,7 +320,7 @@ let _ =
define "AC_ID" ac_id;
define "MD5SUM" (sprintf "((uint8_t*)\"%s\")" (hex_to_bin md5sum));
nl ();
- List.iter parse_section (Xml.children xml);
+ List.iter (parse_section ac_id) (Xml.children xml);
finish h_name
with
Xml.Error e -> fprintf stderr "%s: XML error:%s\n" xml_file (Xml.error e); exit 1
diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml
index cdcdad33ac..972704966b 100644
--- a/sw/tools/gen_flight_plan.ml
+++ b/sw/tools/gen_flight_plan.ml
@@ -794,6 +794,15 @@ let () =
Xml2h.define "HOME_MODE_HEIGHT" (sof home_mode_height);
Xml2h.define "MAX_DIST_FROM_HOME" (sof mdfh);
+ (** Print defines for blocks **)
+ lprintf "\n";
+ let idx = ref 0 in
+ List.iter
+ (fun s ->
+ let v = ExtXml.attrib s "name" in
+ lprintf "#define BLOCK_%s %d\n" (Str.global_replace (Str.regexp "[\\. ]") "_" v) !idx; incr idx) blocks;
+ lprintf "\n";
+
let index_of_waypoints =
let i = ref (-1) in
List.map (fun w -> incr i; (name_of w, !i)) waypoints in
diff --git a/sw/tools/gen_modules.ml b/sw/tools/gen_modules.ml
index 1b4db5371f..7a32fcfe1c 100644
--- a/sw/tools/gen_modules.ml
+++ b/sw/tools/gen_modules.ml
@@ -344,9 +344,10 @@ let write_settings = fun xml_file out_set modules ->
let h_name = "MODULES_H"
let () =
- if Array.length Sys.argv <> 3 then
- failwith (Printf.sprintf "Usage: %s out_settings_file xml_file" Sys.argv.(0));
- let xml_file = Sys.argv.(2)
+ if Array.length Sys.argv <> 4 then
+ failwith (Printf.sprintf "Usage: %s out_settings_file default_freq xml_file" Sys.argv.(0));
+ let xml_file = Sys.argv.(3)
+ and default_freq = int_of_string(Sys.argv.(2))
and out_set = open_out Sys.argv.(1) in
try
let xml = start_and_begin xml_file h_name in
@@ -355,16 +356,18 @@ let () =
fprintf out_h "#define MODULES_START 2\n";
fprintf out_h "#define MODULES_STOP 3\n";
nl ();
+ (* Extract main_freq parameter *)
+ let modules = try (ExtXml.child xml "modules") with _ -> Xml.Element("modules",[],[]) in
+ let main_freq = try (int_of_string (Xml.attrib modules "main_freq")) with _ -> default_freq in
+ freq := main_freq;
+ fprintf out_h "#define MODULES_FREQUENCY %d\n" !freq;
+ nl ();
fprintf out_h "#ifdef MODULES_C\n";
fprintf out_h "#define EXTERN_MODULES\n";
fprintf out_h "#else\n";
fprintf out_h "#define EXTERN_MODULES extern\n";
fprintf out_h "#endif";
nl ();
- (* Extract main_freq parameter *)
- let modules = try (ExtXml.child xml "modules") with _ -> Xml.Element("modules",[],[]) in
- let main_freq = try (int_of_string (Xml.attrib modules "main_freq")) with _ -> !freq in
- freq := main_freq;
(* Extract modules list *)
let modules = GC.get_modules_of_airframe xml in
let modules = GC.unload_unused_modules modules true in
diff --git a/sw/tools/gen_periodic.ml b/sw/tools/gen_periodic.ml
index 1de08f34c8..f70e5ede37 100644
--- a/sw/tools/gen_periodic.ml
+++ b/sw/tools/gen_periodic.ml
@@ -149,6 +149,7 @@ let _ =
fprintf out_h "#include \"std.h\"\n";
fprintf out_h "#include \"generated/airframe.h\"\n";
fprintf out_h "#include \"subsystems/datalink/telemetry_common.h\"\n\n";
+ fprintf out_h "#define TELEMETRY_FREQUENCY %d\n\n" freq;
(** For each process *)
List.iter
diff --git a/sw/tools/gen_settings.ml b/sw/tools/gen_settings.ml
index 32313bd37a..087d477f22 100644
--- a/sw/tools/gen_settings.ml
+++ b/sw/tools/gen_settings.ml
@@ -68,6 +68,16 @@ let print_dl_settings = fun settings ->
lprintf "#include \"generated/periodic_telemetry.h\"\n";
lprintf "\n";
+ (** Datalink knowing what settings mean **)
+ let idx = ref 0 in
+ lprintf "\n";
+ List.iter
+ (fun s ->
+ let v = ExtXml.attrib s "var" in
+ lprintf "#define SETTINGS_%s %d\n" (Str.global_replace (Str.regexp "\\.") "_" v) !idx; incr idx)
+ settings;
+ lprintf "\n";
+
(** Macro to call to set one variable *)
lprintf "#define DlSetting(_idx, _value) { \\\n";
right ();
diff --git a/tests/examples/01_compile_all_test_targets.t b/tests/examples/01_compile_all_test_targets.t
index f18feddaa5..03bab305bf 100644
--- a/tests/examples/01_compile_all_test_targets.t
+++ b/tests/examples/01_compile_all_test_targets.t
@@ -8,7 +8,7 @@ use Data::Dumper;
use Config;
$|++;
-my $examples = XMLin("$ENV{'PAPARAZZI_SRC'}/conf/tests_conf.xml");
+my $examples = XMLin("$ENV{'PAPARAZZI_SRC'}/conf/conf_tests.xml");
use Data::Dumper;