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..9cc359d7cd 100644 --- a/conf/Makefile.omap +++ b/conf/Makefile.omap @@ -78,43 +78,80 @@ 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)"; \ } | 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 $(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 "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 + +# Program the device and start it. +load2 upload2 program2: $(OBJDIR)/$(TARGET).elf + + # Kill the application + -echo "killall -9 $(TARGET).elf" | telnet $(HOST) + + # Make the target dir and edit the config + -{ \ + 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/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 "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. .SECONDARY : $(OBJDIR)/$(TARGET).elf @@ -137,6 +174,7 @@ $(OBJDIR)/%.o : %.cpp $(OBJDIR)/../Makefile.ac $(Q)test -d $(dir $@) || mkdir -p $(dir $@) $(Q)$(CXX) -c $(CXXFLAGS) $< -o $@ + # Listing of phony targets. .PHONY : all build elf clean clean_list 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/apogee.xml b/conf/airframes/ENAC/fixed-wing/apogee.xml index afe5f8a96f..5c62d51f53 100644 --- a/conf/airframes/ENAC/fixed-wing/apogee.xml +++ b/conf/airframes/ENAC/fixed-wing/apogee.xml @@ -8,6 +8,9 @@ + + + @@ -20,24 +23,24 @@ - + + - + - - - + + diff --git a/conf/airframes/ENAC/fixed-wing/eternity1.xml b/conf/airframes/ENAC/fixed-wing/eternity1.xml index 6a9595967d..06bd7fc9d1 100644 --- a/conf/airframes/ENAC/fixed-wing/eternity1.xml +++ b/conf/airframes/ENAC/fixed-wing/eternity1.xml @@ -26,9 +26,7 @@ - - - + diff --git a/conf/airframes/ENAC/fixed-wing/firestorm.xml b/conf/airframes/ENAC/fixed-wing/firestorm.xml index 584e3fb3a9..222712c259 100644 --- a/conf/airframes/ENAC/fixed-wing/firestorm.xml +++ b/conf/airframes/ENAC/fixed-wing/firestorm.xml @@ -32,9 +32,7 @@ - - - + diff --git a/conf/airframes/ENAC/fixed-wing/funjet2.xml b/conf/airframes/ENAC/fixed-wing/funjet2.xml index bd474733af..56455152a5 100644 --- a/conf/airframes/ENAC/fixed-wing/funjet2.xml +++ b/conf/airframes/ENAC/fixed-wing/funjet2.xml @@ -39,9 +39,7 @@ - - - + diff --git a/conf/airframes/ENAC/fixed-wing/mythe.xml b/conf/airframes/ENAC/fixed-wing/mythe.xml index 5a39cdc4fa..f6c226b11b 100644 --- a/conf/airframes/ENAC/fixed-wing/mythe.xml +++ b/conf/airframes/ENAC/fixed-wing/mythe.xml @@ -66,9 +66,7 @@ - - - + 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/fixed-wing/weasel.xml b/conf/airframes/ENAC/fixed-wing/weasel.xml index 744c25880c..f15a4b5c41 100644 --- a/conf/airframes/ENAC/fixed-wing/weasel.xml +++ b/conf/airframes/ENAC/fixed-wing/weasel.xml @@ -20,26 +20,27 @@ - + - - - + - + + + + + + - - - - + + @@ -49,17 +50,11 @@ - - - - - - - - + + @@ -83,13 +78,13 @@ - - + +
- - + +
@@ -123,8 +118,8 @@
- - + +
@@ -161,15 +156,15 @@ - - - + + + - - - + + + @@ -206,8 +201,8 @@ - - + + @@ -234,7 +229,7 @@ - + diff --git a/conf/airframes/ENAC/quadrotor/booz2_g1.xml b/conf/airframes/ENAC/quadrotor/booz2_g1.xml index 472c32d189..834132e720 100644 --- a/conf/airframes/ENAC/quadrotor/booz2_g1.xml +++ b/conf/airframes/ENAC/quadrotor/booz2_g1.xml @@ -213,7 +213,7 @@ - +
@@ -260,7 +260,7 @@
- +
diff --git a/conf/airframes/ENAC/quadrotor/hen1.xml b/conf/airframes/ENAC/quadrotor/hen1.xml index 78b81b8e0b..2c0bdb49ae 100644 --- a/conf/airframes/ENAC/quadrotor/hen1.xml +++ b/conf/airframes/ENAC/quadrotor/hen1.xml @@ -3,11 +3,11 @@ - + @@ -23,23 +23,23 @@ - + - + - + - - - - - + + + +
@@ -48,9 +48,9 @@ - - - + + +
@@ -62,25 +62,25 @@ - - - - + + + + - - - + + + - - - - + + + +
@@ -110,9 +110,9 @@ - + - +
@@ -146,76 +146,77 @@
- - - - + + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - + - +
-
+
- - - - - - - - - - - - + + + + + + + + + + + + - + - +
+ - - - + + + @@ -227,9 +228,8 @@
- - - + +
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..8025779613 100644 --- a/conf/airframes/ardrone2_raw.xml +++ b/conf/airframes/ardrone2_raw.xml @@ -4,235 +4,228 @@ - - - - - - - + + + + + + + + - - + + - - - - - - - - - + + + + + + + + - + + + - - - - + + + + - - - - + + + +
- - - - - + + + + + + - - - - + + + +
- - - - - + + + + +
- - - - - - + + + + - - - + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + +
- +
+ + + +
+ +
- - - + + + + + +
- +
- - - - - - - + + + + + + + - - - + + + - - - + + + - - - + + +
-
- - - - - - + + + + + + - - - - + + + + - - - - + + + + - - - - + + + + - - - + + + - - - + + + - - - + + + - - - + + +
- - - - - - - - + + + + + + + + - + - - - + + +
- - - - + + + + + + +
- - - + + +
- - - + + + +
- - - - + + + + +
- diff --git a/conf/airframes/ardrone2_sdk.xml b/conf/airframes/ardrone2_sdk.xml index 7544a9a391..73c21af476 100644 --- a/conf/airframes/ardrone2_sdk.xml +++ b/conf/airframes/ardrone2_sdk.xml @@ -6,6 +6,7 @@ + @@ -84,9 +85,8 @@ - - + +
@@ -97,9 +97,10 @@
+ - - + +
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 39c98a1283..80dd244402 100644 --- a/conf/airframes/examples/MentorEnergy.xml +++ b/conf/airframes/examples/MentorEnergy.xml @@ -9,7 +9,6 @@ - @@ -22,12 +21,16 @@ + + + + + - - + 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/fixedwing/krooz_sd_fw.xml b/conf/airframes/examples/krooz_sd/fixedwing/krooz_sd_fw.xml new file mode 100644 index 0000000000..7c0ab8835c --- /dev/null +++ b/conf/airframes/examples/krooz_sd/fixedwing/krooz_sd_fw.xml @@ -0,0 +1,190 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + +
+ +
+ + +
+ +
+ + + + + + + + +
+ + +
+ + + + + + + + + + + + + + + + + + + + + +
+ + +
+ + + + + + + + + + + + + +
+ +
+ +
+ +
+ + + + + + + + +
+ +
+ + + + + +
+ + diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml old mode 100755 new mode 100644 index 00c0fd5239..44f2681826 --- a/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml @@ -17,7 +17,7 @@ - + @@ -28,10 +28,11 @@ - - + + @@ -80,34 +81,29 @@ -
- - -
-
- - - + + + - - - - - - + + + + + + - - - - - - + + + + + + @@ -123,7 +119,7 @@
- +
@@ -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 old mode 100755 new mode 100644 index 916d281c06..4bf55eeb9f --- a/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml @@ -17,7 +17,7 @@ - + @@ -32,8 +32,9 @@ - + + --> + @@ -83,34 +84,29 @@ -
- - -
-
- - - + + + - - - - - - + + + + + + - - - - - - + + + + + + @@ -126,7 +122,7 @@
- +
@@ -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 old mode 100755 new mode 100644 index c018d28048..ebdda35837 --- a/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml @@ -15,7 +15,7 @@ - + @@ -32,6 +32,7 @@ +
@@ -61,11 +62,6 @@ -
- - -
-
@@ -84,23 +80,23 @@ - - - + + + - - - + + + - - - - - - + + + + + + @@ -116,7 +112,7 @@
- +
@@ -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 old mode 100755 new mode 100644 index 88b83d3cf3..cf85a484c0 --- a/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml @@ -19,7 +19,7 @@ - + @@ -29,9 +29,11 @@ - - + + @@ -56,11 +58,6 @@ -
- - -
-
@@ -79,27 +76,27 @@ - - - + + + - - - - - - + + + + + + - - - - - - + + + + + + - +
@@ -111,7 +108,7 @@
- +
@@ -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 8f583e899b..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 -->
- +
@@ -246,7 +247,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/flixr_discovery.xml b/conf/airframes/flixr_discovery.xml index c19b65dddd..84cc1f6305 100644 --- a/conf/airframes/flixr_discovery.xml +++ b/conf/airframes/flixr_discovery.xml @@ -67,7 +67,7 @@ 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/fixed-wing/slowfast2.xml b/conf/airframes/obsolete/mm/fixed-wing/slowfast2.xml index ce6d0b7f49..8a0c6e8042 100644 --- a/conf/airframes/obsolete/mm/fixed-wing/slowfast2.xml +++ b/conf/airframes/obsolete/mm/fixed-wing/slowfast2.xml @@ -36,7 +36,7 @@ - + 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/apogee_1.0.makefile b/conf/boards/apogee_1.0.makefile new file mode 100644 index 0000000000..07639c1ccf --- /dev/null +++ b/conf/boards/apogee_1.0.makefile @@ -0,0 +1,58 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# apogee_1.0.makefile +# +# + +BOARD=apogee +BOARD_VERSION=1.0 +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 ?= DFU +STLINK ?= y +DFU_UTIL ?= y + +# +# default LED configuration +# +RADIO_CONTROL_LED ?= 4 +BARO_LED ?= none +AHRS_ALIGNER_LED ?= 2 +GPS_LED ?= 3 +SYS_TIME_LED ?= 1 + +# +# default UART configuration (modem, gps, spektrum) +# + +MODEM_PORT ?= UART1 +MODEM_BAUD ?= B57600 + +GPS_PORT ?= UART4 +GPS_BAUD ?= B38400 + +RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT ?= UART2 + +SBUS_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/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 86% rename from conf/conf.xml.example rename to conf/conf_example.xml index d3d1944b63..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" /> + + diff --git a/conf/tests_conf.xml b/conf/conf_tests.xml similarity index 100% rename from conf/tests_conf.xml rename to conf/conf_tests.xml diff --git a/conf/conf.xml.tri b/conf/conf_tri.xml similarity index 100% rename from conf/conf.xml.tri rename to conf/conf_tri.xml diff --git a/conf/control_panel.xml.example b/conf/control_panel_example.xml similarity index 100% rename from conf/control_panel.xml.example rename to conf/control_panel_example.xml diff --git a/conf/firmwares/lisa_test_progs.makefile b/conf/firmwares/lisa_test_progs.makefile index bb8f1a53b8..5a18345359 100644 --- a/conf/firmwares/lisa_test_progs.makefile +++ b/conf/firmwares/lisa_test_progs.makefile @@ -203,16 +203,21 @@ LISA_M_BARO ?= BARO_BOARD_BMP085 ifeq ($(LISA_M_BARO), BARO_MS5611_SPI) include $(CFG_SHARED)/spi_master.makefile test_baro.CFLAGS += -DUSE_SPI2 -DUSE_SPI_SLAVE3 - test_baro.srcs += $(SRC_BOARD)/baro_ms5611_spi.c + test_baro.srcs += peripherals/ms5611.c + test_baro.srcs += peripherals/ms5611_spi.c + test_baro.srcs += subsystems/sensors/baro_ms5611_spi.c test_baro.srcs += $(SRC_LISA)/test_baro_spi.c else ifeq ($(LISA_M_BARO), BARO_MS5611_I2C) test_baro.CFLAGS += -DUSE_I2C2 test_baro.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c - test_baro.srcs += $(SRC_BOARD)/baro_ms5611_i2c.c + test_baro.srcs += peripherals/ms5611.c + test_baro.srcs += peripherals/ms5611_i2c.c + test_baro.srcs += subsystems/sensors/baro_ms5611_i2c.c test_baro.srcs += $(SRC_LISA)/test_baro_i2c.c else ifeq ($(LISA_M_BARO), BARO_BOARD_BMP085) test_baro.CFLAGS += -DUSE_I2C2 test_baro.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c + test_baro.srcs += peripherals/bmp085.c test_baro.srcs += $(SRC_BOARD)/baro_board.c test_baro.srcs += $(SRC_LISA)/test_baro_i2c.c endif diff --git a/conf/firmwares/rotorcraft.makefile b/conf/firmwares/rotorcraft.makefile index e3ae4a8c97..884bc9a178 100644 --- a/conf/firmwares/rotorcraft.makefile +++ b/conf/firmwares/rotorcraft.makefile @@ -84,9 +84,6 @@ endif PERIODIC_FREQUENCY ?= 512 ap.CFLAGS += -DPERIODIC_FREQUENCY=$(PERIODIC_FREQUENCY) -TELEMETRY_FREQUENCY ?= 60 -ap.CFLAGS += -DTELEMETRY_FREQUENCY=$(TELEMETRY_FREQUENCY) - # # Systime # @@ -108,6 +105,9 @@ ap.srcs += $(SRC_ARCH)/subsystems/settings_arch.c ap.srcs += mcu_periph/uart.c ap.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c +ifeq ($(ARCH), omap) +ap.srcs += $(SRC_ARCH)/serial_port.c +endif # I2C is needed for speed controllers and barometers on lisa ifeq ($(TARGET), ap) @@ -169,16 +169,33 @@ LISA_M_BARO ?= BARO_BOARD_BMP085 ifeq ($(LISA_M_BARO), BARO_MS5611_SPI) include $(CFG_SHARED)/spi_master.makefile ap.CFLAGS += -DUSE_SPI2 -DUSE_SPI_SLAVE3 - ap.srcs += $(SRC_BOARD)/baro_ms5611_spi.c + ap.srcs += peripherals/ms5611.c + ap.srcs += peripherals/ms5611_spi.c + ap.srcs += subsystems/sensors/baro_ms5611_spi.c else ifeq ($(LISA_M_BARO), BARO_MS5611_I2C) ap.CFLAGS += -DUSE_I2C2 - ap.srcs += $(SRC_BOARD)/baro_ms5611_i2c.c + ap.srcs += peripherals/ms5611.c + ap.srcs += peripherals/ms5611_i2c.c + ap.srcs += subsystems/sensors/baro_ms5611_i2c.c else ifeq ($(LISA_M_BARO), BARO_BOARD_BMP085) + ap.srcs += peripherals/bmp085.c ap.srcs += $(SRC_BOARD)/baro_board.c - ap.CFLAGS += -DUSE_I2C2 + ap.CFLAGS += -DUSE_I2C2 endif ap.CFLAGS += -D$(LISA_M_BARO) +# Lisa/S baro +else ifeq ($(BOARD), lisa_s) +# defaults to SPI baro MS5611 on the board + include $(CFG_SHARED)/spi_master.makefile + ap.CFLAGS += -DUSE_SPI1 -DUSE_SPI_SLAVE1 + ap.CFLAGS += -DMS5611_SPI_DEV=spi1 + ap.CFLAGS += -DMS5611_SLAVE_DEV=SPI_SLAVE1 + ap.srcs += peripherals/ms5611.c + ap.srcs += peripherals/ms5611_spi.c + ap.srcs += subsystems/sensors/baro_ms5611_spi.c + ap.CFLAGS += -DBARO_MS5611_SPI + # Lia baro (no bmp onboard) else ifeq ($(BOARD), lia) # fixme, reuse the baro drivers in lisa_m dir @@ -186,10 +203,14 @@ LIA_BARO ?= BARO_MS5611_SPI ifeq ($(LIA_BARO), BARO_MS5611_SPI) include $(CFG_SHARED)/spi_master.makefile ap.CFLAGS += -DUSE_SPI2 -DUSE_SPI_SLAVE3 - ap.srcs += boards/lisa_m/baro_ms5611_spi.c + ap.srcs += peripherals/ms5611.c + ap.srcs += peripherals/ms5611_spi.c + ap.srcs += subsystems/sensors/baro_ms5611_spi.c else ifeq ($(LIA_BARO), BARO_MS5611_I2C) ap.CFLAGS += -DUSE_I2C2 - ap.srcs += boards/lisa_m/baro_ms5611_i2c.c + ap.srcs += peripherals/ms5611.c + ap.srcs += peripherals/ms5611_i2c.c + ap.srcs += subsystems/sensors/baro_ms5611_i2c.c endif ap.CFLAGS += -D$(LIA_BARO) @@ -201,6 +222,19 @@ ap.CFLAGS += -DUSE_SPI1 ap.srcs += peripherals/mcp355x.c ap.srcs += $(SRC_BOARD)/baro_board.c +# krooz baro +else ifeq ($(BOARD), krooz) +ap.CFLAGS += -DMS5611_I2C_DEV=i2c2 -DMS5611_SLAVE_ADDR=0xEC +ap.srcs += peripherals/ms5611.c +ap.srcs += peripherals/ms5611_i2c.c +ap.srcs += subsystems/sensors/baro_ms5611_i2c.c + +else ifeq ($(BOARD), px4fmu) +ap.CFLAGS += -DUSE_I2C2 -DMS5611_I2C_DEV=i2c2 +ap.srcs += peripherals/ms5611.c +ap.srcs += peripherals/ms5611_i2c.c +ap.srcs += subsystems/sensors/baro_ms5611_i2c.c + # apogee baro else ifeq ($(BOARD), apogee) ap.CFLAGS += -DUSE_I2C1 @@ -234,7 +268,7 @@ ap.srcs += subsystems/electrical.c else ifeq ($(BOARD)$(BOARD_TYPE), ardronesdk) ap.srcs += $(SRC_BOARD)/electrical_dummy.c else ifeq ($(BOARD)$(BOARD_TYPE), ardroneraw) -ap.srcs += $(SRC_ARCH)/subsystems/electrical/electrical_arch.c +ap.srcs += $(SRC_BOARD)/electrical_raw.c endif @@ -271,6 +305,7 @@ ap.srcs += $(SRC_FIRMWARE)/guidance/guidance_h.c ap.srcs += $(SRC_FIRMWARE)/guidance/guidance_h_ref.c ap.srcs += $(SRC_FIRMWARE)/guidance/guidance_v.c ap.srcs += $(SRC_FIRMWARE)/guidance/guidance_v_ref.c +ap.srcs += $(SRC_FIRMWARE)/guidance/guidance_v_adapt.c # # INS choice @@ -286,17 +321,3 @@ ap.srcs += $(SRC_FIRMWARE)/guidance/guidance_v_ref.c ap.srcs += $(SRC_FIRMWARE)/navigation.c ap.srcs += subsystems/navigation/common_flight_plan.c -# -# FMS choice -# -# include booz2_fms_test_signal.makefile -# or -# include booz2_fms_datalink.makefile -# or -# nothing -# -ifeq ($(ARCH), omap) -SRC_FMS=fms -ap.CFLAGS += -I. -I$(SRC_FMS) -ap.srcs += $(SRC_FMS)/fms_serial_port.c -endif diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_quat.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_quat.makefile index 095cc6fda2..4dc3e79289 100644 --- a/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_quat.makefile +++ b/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_quat.makefile @@ -6,6 +6,7 @@ # USE_MAGNETOMETER ?= 1 +AHRS_ALIGNER_LED ?= none AHRS_CFLAGS = -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR AHRS_CFLAGS += -DUSE_AHRS_ALIGNER -DAHRS_GRAVITY_UPDATE_COORDINATED_TURN @@ -36,6 +37,11 @@ ap.srcs += $(AHRS_SRCS) #ap.CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY) #endif +# +# NPS uses the real algorithm +# +nps.CFLAGS += $(AHRS_CFLAGS) +nps.srcs += $(AHRS_SRCS) # # Simple simulation of the AHRS result diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile index 8a5330bea2..850cd7c4e3 100644 --- a/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile +++ b/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile @@ -6,6 +6,7 @@ # USE_MAGNETOMETER ?= 1 +AHRS_ALIGNER_LED ?= none AHRS_CFLAGS = -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR AHRS_CFLAGS += -DUSE_AHRS_ALIGNER -DAHRS_GRAVITY_UPDATE_COORDINATED_TURN @@ -27,6 +28,11 @@ AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c ap.CFLAGS += $(AHRS_CFLAGS) ap.srcs += $(AHRS_SRCS) +# +# NPS uses the real algorithm +# +nps.CFLAGS += $(AHRS_CFLAGS) +nps.srcs += $(AHRS_SRCS) # # Simple simulation of the AHRS result diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_float_dcm.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_float_dcm.makefile index 26f09e2518..805e38da83 100644 --- a/conf/firmwares/subsystems/fixedwing/ahrs_float_dcm.makefile +++ b/conf/firmwares/subsystems/fixedwing/ahrs_float_dcm.makefile @@ -3,28 +3,34 @@ # attitude estimation for fixedwings via dcm algorithm USE_MAGNETOMETER ?= 0 +AHRS_ALIGNER_LED ?= none -ifeq ($(TARGET), ap) - -ap.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm.h\" -ap.CFLAGS += -DUSE_AHRS_ALIGNER -ap.CFLAGS += -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR +AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm.h\" +AHRS_CFLAGS += -DUSE_AHRS_ALIGNER +AHRS_CFLAGS += -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR ifneq ($(USE_MAGNETOMETER),0) -ap.CFLAGS += -DUSE_MAGNETOMETER +AHRS_CFLAGS += -DUSE_MAGNETOMETER endif -ap.srcs += $(SRC_SUBSYSTEMS)/ahrs.c -ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c -ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_float_dcm.c +AHRS_SRCS += $(SRC_SUBSYSTEMS)/ahrs.c +AHRS_SRCS += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c +AHRS_SRCS += $(SRC_SUBSYSTEMS)/ahrs/ahrs_float_dcm.c ifneq ($(AHRS_ALIGNER_LED),none) - ap.CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) + AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) endif -endif +ap.CFLAGS += $(AHRS_CFLAGS) +ap.srcs += $(AHRS_SRCS) + +# +# NPS uses the real algorithm +# +nps.CFLAGS += $(AHRS_CFLAGS) +nps.srcs += $(AHRS_SRCS) # # Simple simulation of the AHRS result diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_gx3.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_gx3.makefile new file mode 100644 index 0000000000..e7bb2544c4 --- /dev/null +++ b/conf/firmwares/subsystems/fixedwing/ahrs_gx3.makefile @@ -0,0 +1,32 @@ +# Fixedwing AHRS module for GX3 +# 2013, Utah State University, http://aggieair.usu.edu/ + +GX3_PORT ?= UART3 +GX3_BAUD ?= B921600 + +AHRS_ALIGNER_LED ?= none + +AHRS_CFLAGS = -DUSE_AHRS +AHRS_CFLAGS += -DUSE_IMU +AHRS_CFLAGS += -DUSE_IMU_FLOAT +AHRS_CFLAGS += -DUSE_GX3 + +#fixedwings +AHRS_CFLAGS += -DAHRS_UPDATE_FW_ESTIMATOR +AHRS_CFLAGS += -DUSE_AHRS_ALIGNER + +ifneq ($(AHRS_ALIGNER_LED),none) + AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) +endif + +AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_gx3.h\" +AHRS_SRCS += $(SRC_SUBSYSTEMS)/ahrs.c +AHRS_SRCS += $(SRC_SUBSYSTEMS)/imu.c +AHRS_SRCS += subsystems/ahrs/ahrs_gx3.c + +GX3_PORT_LOWER=$(shell echo $(GX3_PORT) | tr A-Z a-z) +AHRS_CFLAGS += -DUSE_$(GX3_PORT) -D$(GX3_PORT)_BAUD=$(GX3_BAUD) +AHRS_CFLAGS += -DUSE_GX3 -DGX3_PORT=$(GX3_PORT_LOWER) + +ap.CFLAGS += $(AHRS_CFLAGS) +ap.srcs += $(AHRS_SRCS) diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_euler.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_euler.makefile index b1f1bb294f..ba7f7c884a 100644 --- a/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_euler.makefile +++ b/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_euler.makefile @@ -2,6 +2,7 @@ # USE_MAGNETOMETER ?= 1 +AHRS_ALIGNER_LED ?= none AHRS_CFLAGS = -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR -DUSE_AHRS_CMPL_EULER AHRS_CFLAGS += -DUSE_AHRS_ALIGNER @@ -22,6 +23,8 @@ AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c ap.CFLAGS += $(AHRS_CFLAGS) ap.srcs += $(AHRS_SRCS) +nps.CFLAGS += $(AHRS_CFLAGS) +nps.srcs += $(AHRS_SRCS) # # Simple simulation of the AHRS result diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_quat.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_quat.makefile index ca4c99ecdb..b81a94d6ab 100644 --- a/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_quat.makefile +++ b/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_quat.makefile @@ -7,6 +7,7 @@ # USE_MAGNETOMETER ?= 1 +AHRS_ALIGNER_LED ?= none AHRS_CFLAGS = -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR AHRS_CFLAGS += -DUSE_AHRS_ALIGNER -DAHRS_GRAVITY_UPDATE_COORDINATED_TURN @@ -36,6 +37,11 @@ ap.srcs += $(AHRS_SRCS) #ap.CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY) #endif +# +# NPS uses the real algorithm +# +nps.CFLAGS += $(AHRS_CFLAGS) +nps.srcs += $(AHRS_SRCS) # # Simple simulation of the AHRS result diff --git a/conf/firmwares/subsystems/fixedwing/autopilot.makefile b/conf/firmwares/subsystems/fixedwing/autopilot.makefile index 3fef9171db..9970e344d7 100644 --- a/conf/firmwares/subsystems/fixedwing/autopilot.makefile +++ b/conf/firmwares/subsystems/fixedwing/autopilot.makefile @@ -52,17 +52,7 @@ endif $(TARGET).CFLAGS += -DTRAFFIC_INFO -# -# LEDs -# -ifneq ($(ARCH), jsbsim) - $(TARGET).CFLAGS += -DUSE_LED -endif -ifneq ($(ARCH), lpc21) - ifneq ($(ARCH), jsbsim) - $(TARGET).srcs += $(SRC_ARCH)/led_hw.c - endif -endif + # # Sys-time @@ -70,9 +60,6 @@ endif PERIODIC_FREQUENCY ?= 60 $(TARGET).CFLAGS += -DPERIODIC_FREQUENCY=$(PERIODIC_FREQUENCY) -TELEMETRY_FREQUENCY ?= 60 -$(TARGET).CFLAGS += -DTELEMETRY_FREQUENCY=$(TELEMETRY_FREQUENCY) - $(TARGET).srcs += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c $(TARGET).CFLAGS += -DUSE_SYS_TIME @@ -119,10 +106,14 @@ ns_srcs += $(SRC_FIRMWARE)/main.c # # LEDs # +SYS_TIME_LED ?= none ns_CFLAGS += -DUSE_LED ifneq ($(SYS_TIME_LED),none) ns_CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED) endif +ifneq ($(ARCH), lpc21) + ns_srcs += $(SRC_ARCH)/led_hw.c +endif # @@ -202,10 +193,9 @@ sim.CFLAGS += -DSITL sim.srcs += $(SRC_ARCH)/sim_ap.c sim.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport -sim.srcs += subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c +sim.srcs += subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/ivy_transport.c -sim.srcs += subsystems/settings.c -sim.srcs += $(SRC_ARCH)/subsystems/settings_arch.c +sim.srcs += $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_adc_generic.c # hack: always compile some of the sim functions, so ocaml sim does not complain about no-existing functions sim.srcs += $(SRC_ARCH)/sim_ahrs.c $(SRC_ARCH)/sim_ir.c @@ -243,10 +233,9 @@ jsbsim.CFLAGS += -I/usr/include $(shell pkg-config glib-2.0 --cflags) jsbsim.LDFLAGS += $(shell pkg-config glib-2.0 --libs) -lglibivy -lm jsbsim.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport -jsbsim.srcs += subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/jsbsim_hw.c $(SRC_ARCH)/jsbsim_ir.c $(SRC_ARCH)/jsbsim_gps.c $(SRC_ARCH)/jsbsim_ahrs.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/jsbsim_transport.c +jsbsim.srcs += subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/ivy_transport.c -jsbsim.srcs += subsystems/settings.c -jsbsim.srcs += $(SRC_ARCH)/subsystems/settings_arch.c +jsbsim.srcs += $(SRC_ARCH)/jsbsim_hw.c $(SRC_ARCH)/jsbsim_ir.c $(SRC_ARCH)/jsbsim_gps.c $(SRC_ARCH)/jsbsim_ahrs.c $(SRC_ARCH)/jsbsim_transport.c ###################################################################### ## diff --git a/conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile b/conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile new file mode 100644 index 0000000000..3587d244df --- /dev/null +++ b/conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile @@ -0,0 +1,68 @@ +# Hey Emacs, this is a -*- makefile -*- + +# +# SITL Simulator +# + +JSBSIM_ROOT ?= /opt/jsbsim +JSBSIM_INC = $(JSBSIM_ROOT)/include/JSBSim +JSBSIM_LIB = $(JSBSIM_ROOT)/lib + +SRC_FIRMWARE=firmwares/fixedwing + +SRC_BOARD=boards/$(BOARD) + +NPSDIR = $(SIMDIR)/nps + + +nps.ARCHDIR = sim + +# include Makefile.nps instead of Makefile.sim +nps.MAKEFILE = nps + +# add normal ap and fbw sources define in autopilot.makefile +nps.CFLAGS += $(fbw_CFLAGS) $(ap_CFLAGS) +nps.srcs += $(fbw_srcs) $(ap_srcs) + +nps.CFLAGS += -DSITL -DUSE_NPS +nps.CFLAGS += $(shell pkg-config glib-2.0 --cflags) +nps.LDFLAGS += $(shell pkg-config glib-2.0 --libs) -lm -lglibivy -lpcre -lgsl -lgslcblas +nps.CFLAGS += -I$(NPSDIR) -I$(SRC_FIRMWARE) -I$(SRC_BOARD) -I../simulator -I$(PAPARAZZI_HOME)/conf/simulator/nps +nps.LDFLAGS += $(shell sdl-config --libs) + +# use the paparazzi-jsbsim package if it is installed, otherwise look for JSBsim under /opt/jsbsim +JSBSIM_PKG ?= $(shell pkg-config JSBSim --exists && echo 'yes') +ifeq ($(JSBSIM_PKG), yes) + nps.CFLAGS += $(shell pkg-config JSBSim --cflags) + nps.LDFLAGS += $(shell pkg-config JSBSim --libs) +else + JSBSIM_PKG = no + nps.CFLAGS += -I$(JSBSIM_INC) + nps.LDFLAGS += -L$(JSBSIM_LIB) -lJSBSim +endif + + +nps.srcs += $(NPSDIR)/nps_main.c \ + $(NPSDIR)/nps_fdm_jsbsim.c \ + $(NPSDIR)/nps_random.c \ + $(NPSDIR)/nps_sensors.c \ + $(NPSDIR)/nps_sensors_utils.c \ + $(NPSDIR)/nps_sensor_gyro.c \ + $(NPSDIR)/nps_sensor_accel.c \ + $(NPSDIR)/nps_sensor_mag.c \ + $(NPSDIR)/nps_sensor_baro.c \ + $(NPSDIR)/nps_sensor_gps.c \ + $(NPSDIR)/nps_electrical.c \ + $(NPSDIR)/nps_atmosphere.c \ + $(NPSDIR)/nps_radio_control.c \ + $(NPSDIR)/nps_radio_control_joystick.c \ + $(NPSDIR)/nps_radio_control_spektrum.c \ + $(NPSDIR)/nps_autopilot_fixedwing.c \ + $(NPSDIR)/nps_ivy_common.c \ + $(NPSDIR)/nps_ivy_fixedwing.c \ + $(NPSDIR)/nps_flightgear.c \ + + +nps.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport +nps.srcs += subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/ivy_transport.c + diff --git a/conf/firmwares/subsystems/fixedwing/gps_mediatek_diy.makefile b/conf/firmwares/subsystems/fixedwing/gps_mediatek_diy.makefile index 7e013732be..ffea6bd274 100644 --- a/conf/firmwares/subsystems/fixedwing/gps_mediatek_diy.makefile +++ b/conf/firmwares/subsystems/fixedwing/gps_mediatek_diy.makefile @@ -2,6 +2,7 @@ # Mediatek MT3329, DIYDrones V1.4/1.6 protocol +GPS_LED ?= none ap.CFLAGS += -DUSE_GPS -DGPS_CONFIGURE -DGPS_USE_LATLONG ap.CFLAGS += -DGPS_LINK=$(GPS_PORT) @@ -23,3 +24,8 @@ sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c jsbsim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" jsbsim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c + +nps.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG +nps.srcs += $(SRC_SUBSYSTEMS)/gps.c +nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" +nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c diff --git a/conf/firmwares/subsystems/fixedwing/gps_nmea.makefile b/conf/firmwares/subsystems/fixedwing/gps_nmea.makefile index cd68d5f1ee..8e4cac87fe 100644 --- a/conf/firmwares/subsystems/fixedwing/gps_nmea.makefile +++ b/conf/firmwares/subsystems/fixedwing/gps_nmea.makefile @@ -2,6 +2,7 @@ # NMEA GPS unit +GPS_LED ?= none ap.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG ap.CFLAGS += -DGPS_LINK=$(GPS_PORT) @@ -23,3 +24,8 @@ sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c jsbsim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" jsbsim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c + +nps.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG +nps.srcs += $(SRC_SUBSYSTEMS)/gps.c +nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" +nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c diff --git a/conf/firmwares/subsystems/fixedwing/gps_skytraq.makefile b/conf/firmwares/subsystems/fixedwing/gps_skytraq.makefile index f34c47628f..d0b49e7ed0 100644 --- a/conf/firmwares/subsystems/fixedwing/gps_skytraq.makefile +++ b/conf/firmwares/subsystems/fixedwing/gps_skytraq.makefile @@ -1,5 +1,7 @@ # Hey Emacs, this is a -*- makefile -*- +GPS_LED ?= none + ap.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG ap.CFLAGS += -DGPS_LINK=$(GPS_PORT) ap.CFLAGS += -DUSE_$(GPS_PORT) @@ -20,3 +22,8 @@ sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c jsbsim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" jsbsim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim. + +nps.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG +nps.srcs += $(SRC_SUBSYSTEMS)/gps.c +nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" +nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c diff --git a/conf/firmwares/subsystems/fixedwing/gps_ublox.makefile b/conf/firmwares/subsystems/fixedwing/gps_ublox.makefile index 3b2f2ca3f3..4116d8cfa7 100644 --- a/conf/firmwares/subsystems/fixedwing/gps_ublox.makefile +++ b/conf/firmwares/subsystems/fixedwing/gps_ublox.makefile @@ -1,6 +1,7 @@ # Hey Emacs, this is a -*- makefile -*- # UBlox LEA 5H +GPS_LED ?= none ap.CFLAGS += -DUSE_GPS -DUBX -DGPS_USE_LATLONG ap.CFLAGS += -DGPS_LINK=$(GPS_PORT) @@ -22,3 +23,8 @@ sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c jsbsim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" jsbsim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c + +nps.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG +nps.srcs += $(SRC_SUBSYSTEMS)/gps.c +nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" +nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c diff --git a/conf/firmwares/subsystems/fixedwing/gps_ublox_utm.makefile b/conf/firmwares/subsystems/fixedwing/gps_ublox_utm.makefile index 1680755f90..0a9622ff51 100644 --- a/conf/firmwares/subsystems/fixedwing/gps_ublox_utm.makefile +++ b/conf/firmwares/subsystems/fixedwing/gps_ublox_utm.makefile @@ -1,6 +1,7 @@ # Hey Emacs, this is a -*- makefile -*- # UBlox LEA 4P +GPS_LED ?= none ap.CFLAGS += -DUSE_GPS -DUBX ap.CFLAGS += -DGPS_LINK=$(GPS_PORT) @@ -21,3 +22,8 @@ sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c jsbsim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" jsbsim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c + +nps.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG +nps.srcs += $(SRC_SUBSYSTEMS)/gps.c +nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" +nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c diff --git a/conf/firmwares/subsystems/fixedwing/ins_alt_float.makefile b/conf/firmwares/subsystems/fixedwing/ins_alt_float.makefile index 1abd1c7881..fb75201564 100644 --- a/conf/firmwares/subsystems/fixedwing/ins_alt_float.makefile +++ b/conf/firmwares/subsystems/fixedwing/ins_alt_float.makefile @@ -3,3 +3,7 @@ ap_CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_alt_float.h\" ap_srcs += $(SRC_FIXEDWING)/subsystems/ins.c ap_srcs += $(SRC_FIXEDWING)/subsystems/ins/ins_alt_float.c + +nps.CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_alt_float.h\" +nps.srcs += $(SRC_FIXEDWING)/subsystems/ins.c +nps.srcs += $(SRC_FIXEDWING)/subsystems/ins/ins_alt_float.c diff --git a/conf/firmwares/subsystems/fixedwing/ins_gps_passthrough.makefile b/conf/firmwares/subsystems/fixedwing/ins_gps_passthrough.makefile index 19f3fdf8dc..f6f1c628f9 100644 --- a/conf/firmwares/subsystems/fixedwing/ins_gps_passthrough.makefile +++ b/conf/firmwares/subsystems/fixedwing/ins_gps_passthrough.makefile @@ -2,3 +2,5 @@ ap.srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough.c sim.srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough.c + +nps.srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough.c diff --git a/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile b/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile index 6268afc6be..4440f92bfd 100644 --- a/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile +++ b/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile @@ -46,21 +46,22 @@ ap.srcs += $(SRC_SUBSYSTEMS)/gps.c ######################################### ## Simulator +SIM_TARGETS = sim jsbsim nps -sim.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\" -sim.CFLAGS += -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR +ifneq (,$(findstring $(TARGET),$(SIM_TARGETS))) -sim.srcs += $(SRC_SUBSYSTEMS)/ahrs.c -sim.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c - -sim.srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough.c - -sim.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG -sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" -sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c -sim.srcs += $(SRC_SUBSYSTEMS)/gps.c +$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\" +$(TARGET).CFLAGS += -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ahrs.c +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough.c +$(TARGET).CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG +$(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c +endif diff --git a/conf/firmwares/subsystems/fixedwing/joystick.makefile b/conf/firmwares/subsystems/fixedwing/joystick.makefile deleted file mode 100644 index f0787a5567..0000000000 --- a/conf/firmwares/subsystems/fixedwing/joystick.makefile +++ /dev/null @@ -1,6 +0,0 @@ -# Hey Emacs, this is a -*- makefile -*- - -# joystick for fixedwings - -$(TARGET).CFLAGS += -DUSE_JOYSTICK - diff --git a/conf/firmwares/subsystems/fixedwing/spi_slave_hs.makefile b/conf/firmwares/subsystems/fixedwing/spi_slave_hs.makefile index c1d03e2aa6..af019ea9a3 100644 --- a/conf/firmwares/subsystems/fixedwing/spi_slave_hs.makefile +++ b/conf/firmwares/subsystems/fixedwing/spi_slave_hs.makefile @@ -1,7 +1,7 @@ # Hey Emacs, this is a -*- makefile -*- #generic spi driver -$(TARGET).CFLAGS += -DUSE_SPI +$(TARGET).CFLAGS += -DUSE_SPI -DSPI_SLAVE_HS ifeq ($(TARGET), sim) else diff --git a/conf/firmwares/subsystems/fixedwing/telemetry_superbitrf.makefile b/conf/firmwares/subsystems/fixedwing/telemetry_superbitrf.makefile new file mode 100644 index 0000000000..95e0156b6c --- /dev/null +++ b/conf/firmwares/subsystems/fixedwing/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_FBW_DEVICE=SuperbitRF -DDOWNLINK_AP_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 diff --git a/conf/firmwares/subsystems/lisa_passthrough/booz_stabilization_int.makefile b/conf/firmwares/subsystems/lisa_passthrough/booz_stabilization_int.makefile index 53d7b44202..9798cb93cb 100644 --- a/conf/firmwares/subsystems/lisa_passthrough/booz_stabilization_int.makefile +++ b/conf/firmwares/subsystems/lisa_passthrough/booz_stabilization_int.makefile @@ -14,7 +14,7 @@ stm_passthrough.srcs += $(SRC_SUBSYSTEMS)/ins.c stm_passthrough.srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_geodetic_double.c stm_passthrough.srcs += $(SRC_FIRMWARE)/navigation.c stm_passthrough.srcs += $(SRC_SUBSYSTEMS)/ins/vf_float.c -stm_passthrough.CFLAGS += -DUSE_VFF -DDT_VFILTER='(1./512.)' +stm_passthrough.CFLAGS += -DUSE_VFF stm_passthrough.CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_INT stm_passthrough.CFLAGS += -DSTABILIZATION_ATTITUDE_TYPE_H=\"stabilization/stabilization_attitude_int.h\" diff --git a/conf/firmwares/subsystems/lisa_passthrough/radio_control_spektrum.makefile b/conf/firmwares/subsystems/lisa_passthrough/radio_control_spektrum.makefile index c866fd53e3..49c54b444c 100644 --- a/conf/firmwares/subsystems/lisa_passthrough/radio_control_spektrum.makefile +++ b/conf/firmwares/subsystems/lisa_passthrough/radio_control_spektrum.makefile @@ -2,6 +2,7 @@ # Makefile for radio_control susbsytem in rotorcraft firmware # +RADIO_CONTROL_LED ?= none ifndef RADIO_CONTROL_SPEKTRUM_MODEL RADIO_CONTROL_SPEKTRUM_MODEL=\"subsystems/radio_control/spektrum_dx7se.h\" @@ -10,10 +11,12 @@ endif stm_passthrough.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_BIND_IMPL_FUNC=radio_control_spektrum_try_bind stm_passthrough.CFLAGS += -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/spektrum.h\" stm_passthrough.CFLAGS += -DRADIO_CONTROL_SPEKTRUM_MODEL_H=$(RADIO_CONTROL_SPEKTRUM_MODEL) -stm_passthrough.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED) stm_passthrough.CFLAGS += -DRADIO_CONTROL_LINK=$(RADIO_CONTROL_LINK) stm_passthrough.CFLAGS += -DUSE_$(RADIO_CONTROL_LINK) -D$(RADIO_CONTROL_LINK)_BAUD=B115200 stm_passthrough.srcs += $(SRC_SUBSYSTEMS)/radio_control.c \ $(SRC_SUBSYSTEMS)/radio_control/spektrum.c \ $(SRC_ARCH)/subsystems/radio_control/spektrum_arch.c +ifneq ($(RADIO_CONTROL_LED,none)) +stm_passthrough.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED) +endif diff --git a/conf/firmwares/subsystems/rotorcraft/ahrs_extern_quat.makefile b/conf/firmwares/subsystems/rotorcraft/ahrs_extern_quat.makefile deleted file mode 100644 index 9a2087c811..0000000000 --- a/conf/firmwares/subsystems/rotorcraft/ahrs_extern_quat.makefile +++ /dev/null @@ -1,17 +0,0 @@ -# -# AHRS wrapper for AHRS devices, such as GX3 or UM6 -# 2013, Utah State University, http://aggieair.usu.edu/ - -AHRS_CFLAGS = -DUSE_AHRS - -ifneq ($(AHRS_ALIGNER_LED),none) - AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) -endif - -AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_extern_quat.h\" -AHRS_SRCS += subsystems/ahrs.c -AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c -AHRS_SRCS += subsystems/ahrs/ahrs_extern_quat.c - -ap.CFLAGS += $(AHRS_CFLAGS) -ap.srcs += $(AHRS_SRCS) diff --git a/conf/firmwares/subsystems/rotorcraft/ahrs_float_cmpl_quat.makefile b/conf/firmwares/subsystems/rotorcraft/ahrs_float_cmpl_quat.makefile index 4e1cf5090d..6834e76f67 100644 --- a/conf/firmwares/subsystems/rotorcraft/ahrs_float_cmpl_quat.makefile +++ b/conf/firmwares/subsystems/rotorcraft/ahrs_float_cmpl_quat.makefile @@ -6,8 +6,9 @@ # USE_MAGNETOMETER ?= 1 +AHRS_ALIGNER_LED ?= none -AHRS_CFLAGS = -DUSE_AHRS +AHRS_CFLAGS = -DUSE_AHRS -DAHRS_FLOAT AHRS_CFLAGS += -DUSE_AHRS_ALIGNER ifneq ($(USE_MAGNETOMETER),0) diff --git a/conf/firmwares/subsystems/rotorcraft/ahrs_float_cmpl_rmat.makefile b/conf/firmwares/subsystems/rotorcraft/ahrs_float_cmpl_rmat.makefile index f8800de866..779c55006f 100644 --- a/conf/firmwares/subsystems/rotorcraft/ahrs_float_cmpl_rmat.makefile +++ b/conf/firmwares/subsystems/rotorcraft/ahrs_float_cmpl_rmat.makefile @@ -7,7 +7,7 @@ USE_MAGNETOMETER ?= 1 -AHRS_CFLAGS = -DUSE_AHRS +AHRS_CFLAGS = -DUSE_AHRS -DAHRS_FLOAT AHRS_CFLAGS += -DUSE_AHRS_ALIGNER ifneq ($(USE_MAGNETOMETER),0) diff --git a/conf/firmwares/subsystems/rotorcraft/ahrs_float_mlkf.makefile b/conf/firmwares/subsystems/rotorcraft/ahrs_float_mlkf.makefile index 13049dac9f..e3ee599098 100644 --- a/conf/firmwares/subsystems/rotorcraft/ahrs_float_mlkf.makefile +++ b/conf/firmwares/subsystems/rotorcraft/ahrs_float_mlkf.makefile @@ -3,6 +3,7 @@ # USE_MAGNETOMETER ?= 1 +AHRS_ALIGNER_LED ?= none AHRS_CFLAGS = -DUSE_AHRS -DAHRS_FLOAT AHRS_CFLAGS += -DUSE_AHRS_ALIGNER diff --git a/conf/firmwares/subsystems/shared/ahrs_gx3.makefile b/conf/firmwares/subsystems/rotorcraft/ahrs_gx3.makefile similarity index 75% rename from conf/firmwares/subsystems/shared/ahrs_gx3.makefile rename to conf/firmwares/subsystems/rotorcraft/ahrs_gx3.makefile index 8ac0680b76..e12acb6c6a 100644 --- a/conf/firmwares/subsystems/shared/ahrs_gx3.makefile +++ b/conf/firmwares/subsystems/rotorcraft/ahrs_gx3.makefile @@ -1,12 +1,14 @@ -# AHRS module for GX3 +# Rotorcraft AHRS module for GX3 # 2013, Utah State University, http://aggieair.usu.edu/ GX3_PORT ?= UART3 GX3_BAUD ?= B921600 +AHRS_ALIGNER_LED ?= none AHRS_CFLAGS = -DUSE_AHRS AHRS_CFLAGS += -DUSE_IMU AHRS_CFLAGS += -DUSE_IMU_FLOAT +AHRS_CFLAGS += -DUSE_GX3 ifneq ($(AHRS_ALIGNER_LED),none) AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) @@ -17,8 +19,9 @@ AHRS_SRCS += $(SRC_SUBSYSTEMS)/ahrs.c AHRS_SRCS += $(SRC_SUBSYSTEMS)/imu.c AHRS_SRCS += subsystems/ahrs/ahrs_gx3.c +GX3_PORT_LOWER=$(shell echo $(GX3_PORT) | tr A-Z a-z) AHRS_CFLAGS += -DUSE_$(GX3_PORT) -D$(GX3_PORT)_BAUD=$(GX3_BAUD) -AHRS_CFLAGS += -DUSE_GX3 -DGX3_LINK=$(GX3_PORT) +AHRS_CFLAGS += -DUSE_GX3 -DGX3_PORT=$(GX3_PORT_LOWER) ap.CFLAGS += $(AHRS_CFLAGS) ap.srcs += $(AHRS_SRCS) diff --git a/conf/firmwares/subsystems/rotorcraft/ahrs_int_cmpl_euler.makefile b/conf/firmwares/subsystems/rotorcraft/ahrs_int_cmpl_euler.makefile index 2be240c0b0..1efba93150 100644 --- a/conf/firmwares/subsystems/rotorcraft/ahrs_int_cmpl_euler.makefile +++ b/conf/firmwares/subsystems/rotorcraft/ahrs_int_cmpl_euler.makefile @@ -3,9 +3,8 @@ # Fixed point complementary filter using euler angles for attitude estimation # -ifndef USE_MAGNETOMETER -USE_MAGNETOMETER = 1 -endif +USE_MAGNETOMETER ?= 1 +AHRS_ALIGNER_LED ?= none AHRS_CFLAGS = -DUSE_AHRS -DUSE_AHRS_CMPL_EULER AHRS_CFLAGS += -DUSE_AHRS_ALIGNER diff --git a/conf/firmwares/subsystems/rotorcraft/ahrs_lkf.makefile b/conf/firmwares/subsystems/rotorcraft/ahrs_lkf.makefile deleted file mode 100644 index 2c942a23cf..0000000000 --- a/conf/firmwares/subsystems/rotorcraft/ahrs_lkf.makefile +++ /dev/null @@ -1,13 +0,0 @@ -# -# Error State Space Kalman filter for attitude estimation -# - -ap.CFLAGS += -DUSE_AHRS_LKF -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) -ap.srcs += $(SRC_SUBSYSTEMS)/ahrs.c -ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c -ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_float_lkf.c - -nps.CFLAGS += -DUSE_AHRS_LKF -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) -nps.srcs += $(SRC_SUBSYSTEMS)/ahrs.c -nps.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c -nps.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_float_lkf.c diff --git a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile index f57c246e8c..8db11612fd 100644 --- a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile +++ b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile @@ -38,7 +38,7 @@ else endif -nps.srcs += $(NPSDIR)/nps_main.c \ +nps.srcs += $(NPSDIR)/nps_main.c \ $(NPSDIR)/nps_fdm_jsbsim.c \ $(NPSDIR)/nps_random.c \ $(NPSDIR)/nps_sensors.c \ @@ -48,29 +48,30 @@ nps.srcs += $(NPSDIR)/nps_main.c \ $(NPSDIR)/nps_sensor_mag.c \ $(NPSDIR)/nps_sensor_baro.c \ $(NPSDIR)/nps_sensor_gps.c \ + $(NPSDIR)/nps_electrical.c \ + $(NPSDIR)/nps_atmosphere.c \ $(NPSDIR)/nps_radio_control.c \ $(NPSDIR)/nps_radio_control_joystick.c \ $(NPSDIR)/nps_radio_control_spektrum.c \ - $(NPSDIR)/nps_autopilot_rotorcraft.c \ - $(NPSDIR)/nps_ivy.c \ + $(NPSDIR)/nps_autopilot_rotorcraft.c \ + $(NPSDIR)/nps_ivy_common.c \ + $(NPSDIR)/nps_ivy_rotorcraft.c \ $(NPSDIR)/nps_flightgear.c \ nps.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -DPERIPHERALS_AUTO_INIT -nps.srcs += firmwares/rotorcraft/main.c -nps.srcs += mcu.c -nps.srcs += $(SRC_ARCH)/mcu_arch.c +nps.srcs += firmwares/rotorcraft/main.c +nps.srcs += mcu.c +nps.srcs += $(SRC_ARCH)/mcu_arch.c nps.srcs += mcu_periph/i2c.c nps.srcs += $(SRC_ARCH)/mcu_periph/i2c_arch.c PERIODIC_FREQUENCY ?= 512 -TELEMETRY_FREQUENCY ?= 60 nps.CFLAGS += -DPERIODIC_FREQUENCY=$(PERIODIC_FREQUENCY) -nps.CFLAGS += -DTELEMETRY_FREQUENCY=$(TELEMETRY_FREQUENCY) #nps.CFLAGS += -DUSE_LED nps.srcs += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c @@ -103,12 +104,6 @@ nps.srcs += $(SRC_FIRMWARE)/autopilot.c nps.srcs += state.c -# -# in makefile section of airframe xml -# include $(CFG_BOOZ)/subsystems/booz2_ahrs_lkf.makefile -# or -# include $(CFG_BOOZ)/subsystems/booz2_ahrs_cmpl.makefile -# nps.srcs += $(SRC_FIRMWARE)/stabilization.c nps.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_rate.c @@ -119,17 +114,8 @@ nps.srcs += $(SRC_FIRMWARE)/guidance/guidance_h.c nps.srcs += $(SRC_FIRMWARE)/guidance/guidance_h_ref.c nps.srcs += $(SRC_FIRMWARE)/guidance/guidance_v.c nps.srcs += $(SRC_FIRMWARE)/guidance/guidance_v_ref.c +nps.srcs += $(SRC_FIRMWARE)/guidance/guidance_v_adapt.c -# -# INS choice -# -# include subsystems/rotorcraft/ins.makefile -# or -# include subsystems/rotorcraft/ins_extended.makefile -# -# extra: -# include subsystems/rotorcraft/ins_hff.makefile -# nps.srcs += $(SRC_FIRMWARE)/navigation.c nps.srcs += $(SRC_SUBSYSTEMS)/navigation/common_flight_plan.c diff --git a/conf/firmwares/subsystems/rotorcraft/gps_ardrone2.makefile b/conf/firmwares/subsystems/rotorcraft/gps_ardrone2.makefile index fad4a3919f..49de927e45 100644 --- a/conf/firmwares/subsystems/rotorcraft/gps_ardrone2.makefile +++ b/conf/firmwares/subsystems/rotorcraft/gps_ardrone2.makefile @@ -2,6 +2,7 @@ # ARDrone 2 Flightrecorder GPS unit +GPS_LED ?= none ap.CFLAGS += -DUSE_GPS -DUSE_GPS_ARDRONE2 diff --git a/conf/firmwares/subsystems/rotorcraft/gps_sirf.makefile b/conf/firmwares/subsystems/rotorcraft/gps_sirf.makefile index 3cff8b7fa9..37557f098b 100644 --- a/conf/firmwares/subsystems/rotorcraft/gps_sirf.makefile +++ b/conf/firmwares/subsystems/rotorcraft/gps_sirf.makefile @@ -2,6 +2,8 @@ # Sirf GPS unit +GPS_LED ?= none + ap.CFLAGS += -DUSE_GPS ap.CFLAGS += -DGPS_LINK=$(GPS_PORT) ap.CFLAGS += -DUSE_$(GPS_PORT) diff --git a/conf/firmwares/subsystems/rotorcraft/gps_skytraq.makefile b/conf/firmwares/subsystems/rotorcraft/gps_skytraq.makefile index 57ee37fb20..39404dd9e1 100644 --- a/conf/firmwares/subsystems/rotorcraft/gps_skytraq.makefile +++ b/conf/firmwares/subsystems/rotorcraft/gps_skytraq.makefile @@ -1,5 +1,7 @@ # Hey Emacs, this is a -*- makefile -*- +GPS_LED ?= none + ap.CFLAGS += -DUSE_GPS ap.CFLAGS += -DGPS_LINK=$(GPS_PORT) ap.CFLAGS += -DUSE_$(GPS_PORT) diff --git a/conf/firmwares/subsystems/rotorcraft/gps_ublox.makefile b/conf/firmwares/subsystems/rotorcraft/gps_ublox.makefile index c99d67ba37..0fcd453255 100644 --- a/conf/firmwares/subsystems/rotorcraft/gps_ublox.makefile +++ b/conf/firmwares/subsystems/rotorcraft/gps_ublox.makefile @@ -1,5 +1,7 @@ # Hey Emacs, this is a -*- makefile -*- +GPS_LED ?= none + ap.srcs += $(SRC_SUBSYSTEMS)/gps.c ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_ubx.h\" ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_ubx.c diff --git a/conf/firmwares/subsystems/rotorcraft/ins.makefile b/conf/firmwares/subsystems/rotorcraft/ins.makefile index c23c0333ca..fd4878813c 100644 --- a/conf/firmwares/subsystems/rotorcraft/ins.makefile +++ b/conf/firmwares/subsystems/rotorcraft/ins.makefile @@ -8,5 +8,5 @@ $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_int.c # vertical filter float version $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/vf_float.c -$(TARGET).CFLAGS += -DUSE_VFF -DDT_VFILTER='(1./$(PERIODIC_FREQUENCY).)' +$(TARGET).CFLAGS += -DUSE_VFF diff --git a/conf/firmwares/subsystems/rotorcraft/ins_extended.makefile b/conf/firmwares/subsystems/rotorcraft/ins_extended.makefile index 4928c4c495..6ce7f94af9 100644 --- a/conf/firmwares/subsystems/rotorcraft/ins_extended.makefile +++ b/conf/firmwares/subsystems/rotorcraft/ins_extended.makefile @@ -2,11 +2,11 @@ # extended INS with vertical filter using sonar in a better way (flap ground) # -$(TARGET).CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_int.h\" +$(TARGET).CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_int_extended.h\" $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins.c $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_int_extended.c # vertical filter float version $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/vf_extended_float.c -$(TARGET).CFLAGS += -DUSE_VFF_EXTENDED -DDT_VFILTER='(1./$(PERIODIC_FREQUENCY).)' +$(TARGET).CFLAGS += -DUSE_VFF_EXTENDED diff --git a/conf/firmwares/subsystems/rotorcraft/ins_hff.makefile b/conf/firmwares/subsystems/rotorcraft/ins_hff.makefile index 0e1d9dc5ef..fd0ca65228 100644 --- a/conf/firmwares/subsystems/rotorcraft/ins_hff.makefile +++ b/conf/firmwares/subsystems/rotorcraft/ins_hff.makefile @@ -8,7 +8,7 @@ $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_int.c # vertical filter float version $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/vf_float.c -$(TARGET).CFLAGS += -DUSE_VFF -DDT_VFILTER='(1./$(PERIODIC_FREQUENCY).)' +$(TARGET).CFLAGS += -DUSE_VFF # horizontal filter float version $(TARGET).CFLAGS += -DUSE_HFF diff --git a/conf/firmwares/subsystems/rotorcraft/stabilization_euler.makefile b/conf/firmwares/subsystems/rotorcraft/stabilization_euler.makefile index 70d8683f79..ee591b2a52 100644 --- a/conf/firmwares/subsystems/rotorcraft/stabilization_euler.makefile +++ b/conf/firmwares/subsystems/rotorcraft/stabilization_euler.makefile @@ -1,3 +1,2 @@ -$(warning Warning: The stabilization euler subsystem has been renamed, please replace 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.1.makefile b/conf/firmwares/subsystems/shared/imu_aspirin_v2.1.makefile index 1e16703269..bcadf223b4 100644 --- a/conf/firmwares/subsystems/shared/imu_aspirin_v2.1.makefile +++ b/conf/firmwares/subsystems/shared/imu_aspirin_v2.1.makefile @@ -36,43 +36,7 @@ # # - -# 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 - -# Magnetometer -#IMU_ASPIRIN_2_SRCS += peripherals/hmc58xx.c - -include $(CFG_SHARED)/spi_master.makefile - -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 -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 -endif - -# 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 +include $(CFG_SHARED)/imu_aspirin_v2_common.makefile 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_aspirin_v2.2.makefile b/conf/firmwares/subsystems/shared/imu_aspirin_v2.2.makefile index 99968ac5a2..9a1633ad9f 100644 --- a/conf/firmwares/subsystems/shared/imu_aspirin_v2.2.makefile +++ b/conf/firmwares/subsystems/shared/imu_aspirin_v2.2.makefile @@ -2,13 +2,14 @@ # # Aspirin IMU v2.2 # -# actually identical with v2.1 since baro is not read in IMU driver +# nearly identical with v2.1, only has the MS5611 baro on SPI. +# The Baro CS line is # # # required xml: #
# -# +# # # # @@ -39,4 +40,18 @@ # -include $(CFG_SHARED)/imu_aspirin_v2.1.makefile +include $(CFG_SHARED)/imu_aspirin_v2_common.makefile + +# +# Baro is connected via SPI, so additionally specify the slave select line for it, +# so that it will be unselected at init (baro CS line high) +# +ifeq ($(ARCH), lpc21) +#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 +endif + +ap.CFLAGS += $(IMU_ASPIRIN_2_CFLAGS) +ap.srcs += $(IMU_ASPIRIN_2_SRCS) diff --git a/conf/firmwares/subsystems/shared/imu_aspirin_v2_common.makefile b/conf/firmwares/subsystems/shared/imu_aspirin_v2_common.makefile new file mode 100644 index 0000000000..f8b3560ed5 --- /dev/null +++ b/conf/firmwares/subsystems/shared/imu_aspirin_v2_common.makefile @@ -0,0 +1,91 @@ +# 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 + +# +# SPI device and slave select defaults +# +ifeq ($(ARCH), lpc21) +ASPIRIN_2_SPI_DEV ?= spi1 +ASPIRIN_2_SPI_SLAVE_IDX ?= SPI_SLAVE0 +else ifeq ($(ARCH), stm32) +# Slave select configuration +# SLAVE2 is on PB12 (NSS) (MPU600 CS) +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 + + +# +# NPS simulator +# +include $(CFG_SHARED)/imu_nps.makefile 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 new file mode 100644 index 0000000000..baf9c6ae10 --- /dev/null +++ b/conf/firmwares/subsystems/shared/imu_krooz_sd.makefile @@ -0,0 +1,32 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# KroozSD IMU +# + +IMU_KROOZ_CFLAGS = -DUSE_IMU +IMU_KROOZ_CFLAGS += -DIMU_TYPE_H=\"boards/krooz/imu_krooz.h\" + +IMU_KROOZ_SRCS = $(SRC_SUBSYSTEMS)/imu.c \ + $(SRC_BOARD)/imu_krooz.c \ + $(SRC_ARCH)/subsystems/imu/imu_krooz_sd_arch.c + +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_SRCS += peripherals/mpu60x0.c +IMU_KROOZ_SRCS += peripherals/mpu60x0_i2c.c +IMU_KROOZ_SRCS += peripherals/hmc58xx.c + +AHRS_PROPAGATE_FREQUENCY ?= 512 +AHRS_CORRECT_FREQUENCY ?= 512 +ap.CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=$(AHRS_PROPAGATE_FREQUENCY) +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_aspirin_v2.1_old.makefile b/conf/firmwares/subsystems/shared/imu_px4fmu_v1.7.makefile similarity index 71% rename from conf/firmwares/subsystems/shared/imu_aspirin_v2.1_old.makefile rename to conf/firmwares/subsystems/shared/imu_px4fmu_v1.7.makefile index 446b84af59..68a7bc0099 100644 --- a/conf/firmwares/subsystems/shared/imu_aspirin_v2.1_old.makefile +++ b/conf/firmwares/subsystems/shared/imu_px4fmu_v1.7.makefile @@ -1,12 +1,12 @@ # Hey Emacs, this is a -*- makefile -*- # -# Aspirin IMU v2.1 +# MPU6000 via SPI and HMC5883 via I2C on the PX4FMU v1.7 board # # # required xml: #
# -# +# # # # @@ -39,32 +39,29 @@ # for fixedwing firmware and ap only ifeq ($(TARGET), ap) - IMU_ASPIRIN_CFLAGS = -DUSE_IMU + IMU_PX4FMU_CFLAGS = -DUSE_IMU endif -IMU_ASPIRIN_CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin2.h\" -IMU_ASPIRIN_SRCS = $(SRC_SUBSYSTEMS)/imu.c \ - $(SRC_SUBSYSTEMS)/imu/imu_aspirin2.c - include $(CFG_SHARED)/spi_master.makefile -ifeq ($(ARCH), lpc21) -IMU_ASPIRIN_CFLAGS += -DUSE_SPI1 -IMU_ASPIRIN_CFLAGS += -DUSE_SPI_SLAVE0 -else ifeq ($(ARCH), stm32) -IMU_ASPIRIN_CFLAGS += -DUSE_SPI2 -# Slave select configuration -# SLAVE2 is on PB12 (NSS) (MPU600 CS) -IMU_ASPIRIN_CFLAGS += -DUSE_SPI_SLAVE2 -endif +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 -IMU_ASPIRIN_CFLAGS += -DIMU_ASPIRIN_VERSION_2_1 # 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_ASPIRIN_CFLAGS) -ap.srcs += $(IMU_ASPIRIN_SRCS) +ap.CFLAGS += $(IMU_PX4FMU_CFLAGS) +ap.srcs += $(IMU_PX4FMU_SRCS) # 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 new file mode 100644 index 0000000000..67970f298a --- /dev/null +++ b/conf/firmwares/subsystems/shared/radio_control_sbus.makefile @@ -0,0 +1,23 @@ +# +# Makefile for shared radio_control SBUS subsystem +# + +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) + +$(TARGET).CFLAGS += -DUSE_$(SBUS_PORT_UPPER) -D$(SBUS_PORT_UPPER)_BAUD=B100000 +$(TARGET).CFLAGS += -DSBUS_UART_DEV=$(SBUS_PORT_LOWER) +$(TARGET).CFLAGS += -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/sbus.h\" +$(TARGET).CFLAGS += -DRADIO_CONTROL_TYPE_SBUS +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/radio_control.c +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/radio_control/sbus.c + 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 54% rename from conf/maps.xml.example rename to conf/maps_example.xml index 0103dd0ef4..ea62c97c1e 100644 --- a/conf/maps.xml.example +++ b/conf/maps_example.xml @@ -1,3 +1,3 @@ - + diff --git a/conf/messages.xml b/conf/messages.xml index 919b48adec..29dafa2393 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/airspeed_amsys.xml b/conf/modules/airspeed_amsys.xml index bc1d68ebc1..88c3faf9de 100644 --- a/conf/modules/airspeed_amsys.xml +++ b/conf/modules/airspeed_amsys.xml @@ -8,10 +8,11 @@ (AMS 5812-0003-D) - + + - +
diff --git a/conf/modules/airspeed_ets.xml b/conf/modules/airspeed_ets.xml index de10d509a9..07cf003315 100644 --- a/conf/modules/airspeed_ets.xml +++ b/conf/modules/airspeed_ets.xml @@ -17,11 +17,12 @@ - Yellow wire: SDA - Brown wire: SCL - - - + + + + - +
diff --git a/conf/modules/baro_amsys.xml b/conf/modules/baro_amsys.xml index acd6f98b46..2b94bb226c 100644 --- a/conf/modules/baro_amsys.xml +++ b/conf/modules/baro_amsys.xml @@ -7,6 +7,10 @@ Module to read a Amsys AMS 5812-0150-A barometric sensor via I2C. + + + +
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_ets.xml b/conf/modules/baro_ets.xml index 48b79418a0..0b59c2a45b 100644 --- a/conf/modules/baro_ets.xml +++ b/conf/modules/baro_ets.xml @@ -2,10 +2,26 @@ - Baro ETS (I2C) - - - + + Baro ETS (I2C). + Driver for the EagleTree Systems Baro Sensor. + Has only been tested with V3 of the sensor hardware. + + Notes: + Connect directly to TWOG/Tiny I2C port. Multiple sensors can be chained together. + Sensor should be in the proprietary mode (default) and not in 3rd party mode. + + Sensor module wire assignments: + - Red wire: 5V + - White wire: Ground + - Yellow wire: SDA + - Brown wire: SCL + + + + + +
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 @@
- + + + + + + + + + + + + + + + + diff --git a/conf/settings/control/rotorcraft_guidance.xml b/conf/settings/control/rotorcraft_guidance.xml index 1362a96a38..45c73a958b 100644 --- a/conf/settings/control/rotorcraft_guidance.xml +++ b/conf/settings/control/rotorcraft_guidance.xml @@ -7,7 +7,8 @@ - + + diff --git a/conf/settings/estimation/booz2_ahrs_lkf.xml b/conf/settings/estimation/booz2_ahrs_lkf.xml deleted file mode 100644 index a49a4f318b..0000000000 --- a/conf/settings/estimation/booz2_ahrs_lkf.xml +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - - diff --git a/conf/settings/modules/flight_time.xml b/conf/settings/modules/flight_time.xml new file mode 100644 index 0000000000..312ec12ecc --- /dev/null +++ b/conf/settings/modules/flight_time.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/conf/settings/nps.xml b/conf/settings/nps.xml index d4b2b93c89..ba8aa8f8c6 100644 --- a/conf/settings/nps.xml +++ b/conf/settings/nps.xml @@ -5,6 +5,12 @@ + + + + + + diff --git a/conf/simulator/jsbsim/aircraft/Engines/18x8.xml b/conf/simulator/jsbsim/aircraft/Engines/18x8.xml new file mode 100644 index 0000000000..3dd84ebec3 --- /dev/null +++ b/conf/simulator/jsbsim/aircraft/Engines/18x8.xml @@ -0,0 +1,51 @@ + + + + + + 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 + + + + fcs/elevator-pos-rad + + -0.3 + 0.3 + + + -1 + 1 + + fcs/elevator-pos-norm + + + + fcs/aileron-cmd-norm + fcs/roll-trim-cmd-norm + + -1 + 1 + + + + + fcs/roll-trim-sum + + -0.35 + 0.35 + + fcs/left-aileron-pos-rad + + + + -fcs/roll-trim-sum + + -0.35 + 0.35 + + fcs/right-aileron-pos-rad + + + + fcs/left-aileron-pos-rad + + -0.35 + 0.35 + + + -1 + 1 + + fcs/left-aileron-pos-norm + + + + fcs/right-aileron-pos-rad + + -0.35 + 0.35 + + + -1 + 1 + + fcs/right-aileron-pos-norm + + + + fcs/rudder-cmd-norm + fcs/yaw-trim-cmd-norm + + -1 + 1 + + + + + fcs/rudder-command-sum + + -0.35 + 0.35 + + fcs/rudder-pos-rad + + + + fcs/rudder-pos-rad + + -0.35 + 0.35 + + + -1 + 1 + + fcs/rudder-pos-norm + + + + + + + + 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/preferences.d/paparazzi-uav-ppa b/conf/system/preferences.d/paparazzi-uav-ppa new file mode 100644 index 0000000000..0509d67510 --- /dev/null +++ b/conf/system/preferences.d/paparazzi-uav-ppa @@ -0,0 +1,3 @@ +Package: lpc21isp +Pin: version 1.48* +Pin-Priority: 1001 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 13c26a90c6..e13be3e9e8 100644 --- a/data/maps/Makefile +++ b/data/maps/Makefile @@ -21,14 +21,14 @@ $(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 "-----------------------------------------------" && \ exit 1) $(PAPARAZZI_HOME)/conf/maps.xml: $(DATADIR)/maps.google.com - $(eval GOOGLE_VERSION := $(shell grep -E "http://khm[0-9]+.google.com/kh/v=[0-9]+.x26" $(DATADIR)/maps.google.com | sed -E 's#.*http://khm[0-9]+.google.com/kh/v=##;s#.x26.*##')) + $(eval GOOGLE_VERSION := $(shell tr -s '[[:space:]]' '\n' < $(DATADIR)/maps.google.com | grep -E "http[s]?://khm[s]?[0-9]+.google.com/kh/v=[0-9]+.x26" | sed -E 's#.*http[s]?://khm[s]?[0-9]+.google.com/kh/v=##;s#.x26.*##')) $(eval $@_TMP := $(shell $(MKTEMP))) @echo "Updated google maps version to $(GOOGLE_VERSION)" @echo "-----------------------------------------------" diff --git a/data/pictures/Penguin.gif b/data/pictures/Penguin.gif new file mode 100644 index 0000000000..9e73bc08aa Binary files /dev/null and b/data/pictures/Penguin.gif differ 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_arch.c b/sw/airborne/arch/lpc21/mcu_periph/spi_arch.c index 60134d66ca..252ba5fa09 100644 --- a/sw/airborne/arch/lpc21/mcu_periph/spi_arch.c +++ b/sw/airborne/arch/lpc21/mcu_periph/spi_arch.c @@ -382,6 +382,7 @@ __attribute__ ((always_inline)) static inline void SpiSlaveAutomaton(struct spi_ #if SPI_MASTER #if USE_SPI0 +#error "SPI0 is currently not implemented in the mcu_periph/spi HAL for the LPC!" // void spi0_ISR(void) __attribute__((naked)); // 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 125dac1c6f..5d594566c3 100644 --- a/sw/airborne/arch/lpc21/mcu_periph/uart_arch.c +++ b/sw/airborne/arch/lpc21/mcu_periph/uart_arch.c @@ -68,6 +68,10 @@ 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* p __attribute__((unused)), uint8_t bits __attribute__((unused)), uint8_t stop __attribute__((unused)), uint8_t __attribute__((unused)) parity) { + // TBD +} + void uart_transmit(struct uart_periph* p, uint8_t data ) { uint16_t temp; unsigned cpsr; 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_arch.c b/sw/airborne/arch/stm32/mcu_arch.c index d5d8be1b28..8066250927 100644 --- a/sw/airborne/arch/stm32/mcu_arch.c +++ b/sw/airborne/arch/stm32/mcu_arch.c @@ -33,11 +33,7 @@ #include #include #include -#if defined(STM32F1) -#include -#elif defined(STM32F4) -#include -#endif +#include #include #include "std.h" 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 4384d15686..f2b5cc79f4 100644 --- a/sw/airborne/arch/stm32/mcu_periph/spi_arch.c +++ b/sw/airborne/arch/stm32/mcu_periph/spi_arch.c @@ -84,15 +84,17 @@ struct spi_periph_dma { uint32_t spi; ///< SPI peripheral identifier uint32_t spidr; ///< SPI DataRegister address for DMA uint32_t dma; ///< DMA controller base address (DMA1 or DMA2) - uint8_t rx_chan; ///< receive DMA channel number - uint8_t tx_chan; ///< transmit DMA channel number + uint8_t rx_chan; ///< receive DMA channel (or stream on F4) number + uint8_t tx_chan; ///< transmit DMA channel (or stream on F4) number + uint32_t rx_chan_sel; ///< F4 only: actual receive DMA channel number + uint32_t tx_chan_sel; ///< F4 only: actual transmit DMA channel number uint8_t rx_nvic_irq; ///< receive interrupt uint8_t tx_nvic_irq; ///< transmit interrupt uint16_t tx_dummy_buf; ///< dummy tx buffer for receive only cases - bool_t tx_extra_dummy_dma; ///< extra tx dummy dma flag for tx_len < rx_len + bool_t tx_extra_dummy_dma; ///< extra tx dummy dma flag for tx_len < rx_len uint16_t rx_dummy_buf; ///< dummy rx buffer for receive only cases - bool_t rx_extra_dummy_dma; ///< extra rx dummy dma flag for tx_len > rx_len - struct locm3_spi_comm comm; ///< current communication paramters + bool_t rx_extra_dummy_dma; ///< extra rx dummy dma flag for tx_len > rx_len + struct locm3_spi_comm comm; ///< current communication paramters uint8_t comm_sig; ///< comm config signature used to check for changes }; @@ -439,12 +441,17 @@ static void set_comm_from_transaction(struct locm3_spi_comm* c, struct spi_trans static void spi_configure_dma(uint32_t dma, uint8_t chan, uint32_t periph_addr, uint32_t buf_addr, uint16_t len, enum SPIDataSizeSelect dss, bool_t increment) { +#ifdef STM32F1 dma_channel_reset(dma, chan); +#elif defined STM32F4 + dma_stream_reset(dma, chan); +#endif dma_set_peripheral_address(dma, chan, periph_addr); dma_set_memory_address(dma, chan, buf_addr); dma_set_number_of_data(dma, chan, len); /* Set the dma transfer size based on SPI transaction DSS */ +#ifdef STM32F1 if (dss == SPIDss8bit) { dma_set_peripheral_size(dma, chan, DMA_CCR_PSIZE_8BIT); dma_set_memory_size(dma, chan, DMA_CCR_MSIZE_8BIT); @@ -452,6 +459,15 @@ static void spi_configure_dma(uint32_t dma, uint8_t chan, uint32_t periph_addr, dma_set_peripheral_size(dma, chan, DMA_CCR_PSIZE_16BIT); dma_set_memory_size(dma, chan, DMA_CCR_MSIZE_16BIT); } +#elif defined STM32F4 + if (dss == SPIDss8bit) { + dma_set_peripheral_size(dma, chan, DMA_SxCR_PSIZE_8BIT); + dma_set_memory_size(dma, chan, DMA_SxCR_MSIZE_8BIT); + } else { + dma_set_peripheral_size(dma, chan, DMA_SxCR_PSIZE_16BIT); + dma_set_memory_size(dma, chan, DMA_SxCR_MSIZE_16BIT); + } +#endif if (increment) dma_enable_memory_increment_mode(dma, chan); @@ -566,8 +582,14 @@ static void spi_start_dma_transaction(struct spi_periph* periph, struct spi_tran dma->rx_extra_dummy_dma = TRUE; } } +#ifdef STM32F1 dma_set_read_from_peripheral(dma->dma, dma->rx_chan); dma_set_priority(dma->dma, dma->rx_chan, DMA_CCR_PL_VERY_HIGH); +#elif defined STM32F4 + dma_channel_select(dma->dma, dma->rx_chan, dma->rx_chan_sel); + dma_set_transfer_mode(dma->dma, dma->rx_chan, DMA_SxCR_DIR_PERIPHERAL_TO_MEM); + dma_set_priority(dma->dma, dma->rx_chan, DMA_SxCR_PL_VERY_HIGH); +#endif /* @@ -591,17 +613,27 @@ static void spi_start_dma_transaction(struct spi_periph* periph, struct spi_tran dma->tx_extra_dummy_dma = TRUE; } } +#ifdef STM32F1 dma_set_read_from_memory(dma->dma, dma->tx_chan); dma_set_priority(dma->dma, dma->tx_chan, DMA_CCR_PL_MEDIUM); - +#elif defined STM32F4 + dma_channel_select(dma->dma, dma->tx_chan, dma->tx_chan_sel); + dma_set_transfer_mode(dma->dma, dma->tx_chan, DMA_SxCR_DIR_MEM_TO_PERIPHERAL); + dma_set_priority(dma->dma, dma->tx_chan, DMA_SxCR_PL_MEDIUM); +#endif /* Enable DMA transfer complete interrupts. */ dma_enable_transfer_complete_interrupt(dma->dma, dma->rx_chan); dma_enable_transfer_complete_interrupt(dma->dma, dma->tx_chan); /* Enable DMA channels */ +#ifdef STM32F1 dma_enable_channel(dma->dma, dma->rx_chan); dma_enable_channel(dma->dma, dma->tx_chan); +#elif defined STM32F4 + dma_enable_stream(dma->dma, dma->rx_chan); + dma_enable_stream(dma->dma, dma->tx_chan); +#endif /* Enable SPI transfers via DMA */ spi_enable_rx_dma((uint32_t)periph->reg_addr); @@ -620,11 +652,22 @@ void spi1_arch_init(void) { // set dma options spi1_dma.spidr = (uint32_t)&SPI1_DR; +#ifdef STM32F1 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; +#elif defined STM32F4 + spi1_dma.dma = DMA2; + // TODO make a macro to configure this from board/airframe file ? + spi1_dma.rx_chan = DMA_STREAM0; + spi1_dma.tx_chan = DMA_STREAM3; + spi1_dma.rx_chan_sel = DMA_SxCR_CHSEL_3; + spi1_dma.tx_chan_sel = DMA_SxCR_CHSEL_3; + spi1_dma.rx_nvic_irq = NVIC_DMA2_STREAM0_IRQ; + spi1_dma.tx_nvic_irq = NVIC_DMA2_STREAM3_IRQ; +#endif spi1_dma.tx_dummy_buf = 0; spi1_dma.tx_extra_dummy_dma = FALSE; spi1_dma.rx_dummy_buf = 0; @@ -646,12 +689,22 @@ void spi1_arch_init(void) { rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_SPI1EN); // Configure GPIOs: SCK, MISO and MOSI +#ifdef STM32F1 + // TODO configure lisa board files to use gpio_setup_pin_af function gpio_set_mode(GPIO_BANK_SPI1_SCK, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_SPI1_SCK | GPIO_SPI1_MOSI); gpio_set_mode(GPIO_BANK_SPI1_MISO, GPIO_MODE_INPUT, GPIO_CNF_INPUT_FLOAT, GPIO_SPI1_MISO); +#elif defined STM32F4 + gpio_setup_pin_af(SPI1_GPIO_PORT_MISO, SPI1_GPIO_MISO, SPI1_GPIO_AF, FALSE); + gpio_setup_pin_af(SPI1_GPIO_PORT_MOSI, SPI1_GPIO_MOSI, SPI1_GPIO_AF, TRUE); + gpio_setup_pin_af(SPI1_GPIO_PORT_SCK, SPI1_GPIO_SCK, SPI1_GPIO_AF, TRUE); + + gpio_set_output_options(SPI1_GPIO_PORT_MOSI, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, SPI1_GPIO_MOSI); + gpio_set_output_options(SPI1_GPIO_PORT_SCK, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, SPI1_GPIO_SCK); +#endif // reset SPI spi_reset(SPI1); @@ -677,7 +730,11 @@ void spi1_arch_init(void) { spi_set_nss_high(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); @@ -692,10 +749,19 @@ void spi2_arch_init(void) { // set dma options spi2_dma.spidr = (uint32_t)&SPI2_DR; spi2_dma.dma = DMA1; +#ifdef STM32F1 spi2_dma.rx_chan = DMA_CHANNEL4; spi2_dma.tx_chan = DMA_CHANNEL5; spi2_dma.rx_nvic_irq = NVIC_DMA1_CHANNEL4_IRQ; spi2_dma.tx_nvic_irq = NVIC_DMA1_CHANNEL5_IRQ; +#elif defined STM32F4 + spi2_dma.rx_chan = DMA_STREAM3; + spi2_dma.tx_chan = DMA_STREAM4; + spi2_dma.rx_chan_sel = DMA_SxCR_CHSEL_0; + spi2_dma.tx_chan_sel = DMA_SxCR_CHSEL_0; + spi2_dma.rx_nvic_irq = NVIC_DMA1_STREAM3_IRQ; + spi2_dma.tx_nvic_irq = NVIC_DMA1_STREAM4_IRQ; +#endif spi2_dma.tx_dummy_buf = 0; spi2_dma.tx_extra_dummy_dma = FALSE; spi2_dma.rx_dummy_buf = 0; @@ -717,12 +783,22 @@ void spi2_arch_init(void) { rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_SPI2EN); // Configure GPIOs: SCK, MISO and MOSI +#ifdef STM32F1 + // TODO configure lisa board files to use gpio_setup_pin_af function gpio_set_mode(GPIO_BANK_SPI2_SCK, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_SPI2_SCK | GPIO_SPI2_MOSI); gpio_set_mode(GPIO_BANK_SPI2_MISO, GPIO_MODE_INPUT, GPIO_CNF_INPUT_FLOAT, GPIO_SPI2_MISO); +#elif defined STM32F4 + gpio_setup_pin_af(SPI2_GPIO_PORT_MISO, SPI2_GPIO_MISO, SPI2_GPIO_AF, FALSE); + gpio_setup_pin_af(SPI2_GPIO_PORT_MOSI, SPI2_GPIO_MOSI, SPI2_GPIO_AF, TRUE); + gpio_setup_pin_af(SPI2_GPIO_PORT_SCK, SPI2_GPIO_SCK, SPI2_GPIO_AF, TRUE); + + gpio_set_output_options(SPI2_GPIO_PORT_MOSI, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, SPI2_GPIO_MOSI); + gpio_set_output_options(SPI2_GPIO_PORT_SCK, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, SPI2_GPIO_SCK); +#endif // reset SPI spi_reset(SPI2); @@ -747,7 +823,11 @@ void spi2_arch_init(void) { spi_set_nss_high(SPI2); // Enable SPI_2 DMA clock +#ifdef STM32F1 rcc_peripheral_enable_clock(&RCC_AHBENR, RCC_AHBENR_DMA1EN); +#elif defined STM32F4 + rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_DMA1EN); +#endif // Enable SPI2 periph. spi_enable(SPI2); @@ -761,11 +841,21 @@ void spi3_arch_init(void) { // set the default configuration spi3_dma.spidr = (uint32_t)&SPI3_DR; +#ifdef STM32F1 spi3_dma.dma = DMA2; spi3_dma.rx_chan = DMA_CHANNEL1; spi3_dma.tx_chan = DMA_CHANNEL2; spi3_dma.rx_nvic_irq = NVIC_DMA2_CHANNEL1_IRQ; spi3_dma.tx_nvic_irq = NVIC_DMA2_CHANNEL2_IRQ; +#elif defined STM32F4 + spi3_dma.dma = DMA1; + spi3_dma.rx_chan = DMA_STREAM0; + spi3_dma.tx_chan = DMA_STREAM5; + spi3_dma.rx_chan_sel = DMA_SxCR_CHSEL_0; + spi3_dma.tx_chan_sel = DMA_SxCR_CHSEL_0; + spi3_dma.rx_nvic_irq = NVIC_DMA1_STREAM0_IRQ; + spi3_dma.tx_nvic_irq = NVIC_DMA1_STREAM5_IRQ; +#endif spi3_dma.tx_dummy_buf = 0; spi3_dma.tx_extra_dummy_dma = FALSE; spi3_dma.rx_dummy_buf = 0; @@ -787,12 +877,22 @@ void spi3_arch_init(void) { rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_SPI3EN); // Configure GPIOs: SCK, MISO and MOSI +#ifdef STM32F1 + // TODO configure lisa board files to use gpio_setup_pin_af function gpio_set_mode(GPIO_BANK_SPI3_SCK, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_SPI3_SCK | GPIO_SPI3_MOSI); gpio_set_mode(GPIO_BANK_SPI3_MISO, GPIO_MODE_INPUT, GPIO_CNF_INPUT_FLOAT, GPIO_SPI3_MISO); +#elif defined STM32F4 + gpio_setup_pin_af(SPI3_GPIO_PORT_MISO, SPI3_GPIO_MISO, SPI3_GPIO_AF, FALSE); + gpio_setup_pin_af(SPI3_GPIO_PORT_MOSI, SPI3_GPIO_MOSI, SPI3_GPIO_AF, TRUE); + gpio_setup_pin_af(SPI3_GPIO_PORT_SCK, SPI3_GPIO_SCK, SPI3_GPIO_AF, TRUE); + + gpio_set_output_options(SPI3_GPIO_PORT_MOSI, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, SPI3_GPIO_MOSI); + gpio_set_output_options(SPI3_GPIO_PORT_SCK, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, SPI3_GPIO_SCK); +#endif /// @todo disable JTAG so the pins can be used? @@ -819,7 +919,11 @@ void spi3_arch_init(void) { spi_set_nss_high(SPI3); // Enable SPI_3 DMA clock +#ifdef STM32F1 rcc_peripheral_enable_clock(&RCC_AHBENR, RCC_AHBENR_DMA2EN); +#elif defined STM32F4 + rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_DMA1EN); +#endif // Enable SPI3 periph. spi_enable(SPI3); @@ -838,22 +942,40 @@ void spi3_arch_init(void) { *****************************************************************************/ #ifdef USE_SPI1 /// receive transferred over DMA +#ifdef STM32F1 void dma1_channel2_isr(void) { if ((DMA1_ISR & DMA_ISR_TCIF2) != 0) { // clear int pending bit DMA1_IFCR |= DMA_IFCR_CTCIF2; } +#elif defined STM32F4 +void dma2_stream0_isr(void) +{ + if ((DMA2_LISR & DMA_LISR_TCIF0) != 0) { + // clear int pending bit + DMA2_LIFCR |= DMA_LIFCR_CTCIF0; + } +#endif process_rx_dma_interrupt(&spi1); } /// transmit transferred over DMA +#ifdef STM32F1 void dma1_channel3_isr(void) { if ((DMA1_ISR & DMA_ISR_TCIF3) != 0) { // clear int pending bit DMA1_IFCR |= DMA_IFCR_CTCIF3; } +#elif defined STM32F4 +void dma2_stream3_isr(void) +{ + if ((DMA2_LISR & DMA_LISR_TCIF3) != 0) { + // clear int pending bit + DMA2_LIFCR |= DMA_LIFCR_CTCIF3; + } +#endif process_tx_dma_interrupt(&spi1); } @@ -861,22 +983,40 @@ void dma1_channel3_isr(void) #ifdef USE_SPI2 /// receive transferred over DMA +#ifdef STM32F1 void dma1_channel4_isr(void) { if ((DMA1_ISR & DMA_ISR_TCIF4) != 0) { // clear int pending bit DMA1_IFCR |= DMA_IFCR_CTCIF4; } +#elif defined STM32F4 +void dma1_stream3_isr(void) +{ + if ((DMA1_LISR & DMA_LISR_TCIF3) != 0) { + // clear int pending bit + DMA1_LIFCR |= DMA_LIFCR_CTCIF3; + } +#endif process_rx_dma_interrupt(&spi2); } /// transmit transferred over DMA +#ifdef STM32F1 void dma1_channel5_isr(void) { if ((DMA1_ISR & DMA_ISR_TCIF5) != 0) { // clear int pending bit DMA1_IFCR |= DMA_IFCR_CTCIF5; } +#elif defined STM32F4 +void dma1_stream4_isr(void) +{ + if ((DMA1_HISR & DMA_HISR_TCIF4) != 0) { + // clear int pending bit + DMA1_HIFCR |= DMA_HIFCR_CTCIF4; + } +#endif process_tx_dma_interrupt(&spi2); } @@ -884,22 +1024,40 @@ void dma1_channel5_isr(void) #if USE_SPI3 /// receive transferred over DMA +#ifdef STM32F1 void dma2_channel1_isr(void) { if ((DMA2_ISR & DMA_ISR_TCIF1) != 0) { // clear int pending bit DMA2_IFCR |= DMA_IFCR_CTCIF1; } +#elif defined STM32F4 +void dma1_stream0_isr(void) +{ + if ((DMA1_LISR & DMA_LISR_TCIF0) != 0) { + // clear int pending bit + DMA1_LIFCR |= DMA_LIFCR_CTCIF0; + } +#endif process_rx_dma_interrupt(&spi3); } /// transmit transferred over DMA +#ifdef STM32F1 void dma2_channel2_isr(void) { if ((DMA2_ISR & DMA_ISR_TCIF2) != 0) { // clear int pending bit DMA2_IFCR |= DMA_IFCR_CTCIF2; } +#elif defined STM32F4 +void dma1_stream5_isr(void) +{ + if ((DMA1_HISR & DMA_HISR_TCIF5) != 0) { + // clear int pending bit + DMA1_HIFCR |= DMA_HIFCR_CTCIF5; + } +#endif process_tx_dma_interrupt(&spi3); } @@ -917,7 +1075,11 @@ void process_rx_dma_interrupt(struct spi_periph *periph) { spi_disable_rx_dma((uint32_t)periph->reg_addr); /* Disable DMA rx channel */ +#ifdef STM32F1 dma_disable_channel(dma->dma, dma->rx_chan); +#elif defined STM32F4 + dma_disable_stream(dma->dma, dma->rx_chan); +#endif if (dma->rx_extra_dummy_dma) { @@ -935,13 +1097,23 @@ void process_rx_dma_interrupt(struct spi_periph *periph) { spi_configure_dma(dma->dma, dma->rx_chan, (uint32_t)dma->spidr, (uint32_t)&(dma->rx_dummy_buf), len_remaining, trans->dss, FALSE); +#ifdef STM32F1 dma_set_read_from_peripheral(dma->dma, dma->rx_chan); dma_set_priority(dma->dma, dma->rx_chan, DMA_CCR_PL_HIGH); +#elif defined STM32F4 + dma_channel_select(dma->dma, dma->rx_chan, dma->rx_chan_sel); + dma_set_transfer_mode(dma->dma, dma->rx_chan, DMA_SxCR_DIR_PERIPHERAL_TO_MEM); + dma_set_priority(dma->dma, dma->rx_chan, DMA_SxCR_PL_HIGH); +#endif /* Enable DMA transfer complete interrupts. */ dma_enable_transfer_complete_interrupt(dma->dma, dma->rx_chan); /* Enable DMA channels */ +#ifdef STM32F1 dma_enable_channel(dma->dma, dma->rx_chan); +#elif defined STM32F4 + dma_enable_stream(dma->dma, dma->rx_chan); +#endif /* Enable SPI transfers via DMA */ spi_enable_rx_dma((uint32_t)periph->reg_addr); } @@ -979,7 +1151,11 @@ void process_tx_dma_interrupt(struct spi_periph *periph) { spi_disable_tx_dma((uint32_t)periph->reg_addr); /* Disable DMA tx channel */ +#ifdef STM32F1 dma_disable_channel(dma->dma, dma->tx_chan); +#elif defined STM32F4 + dma_disable_stream(dma->dma, dma->tx_chan); +#endif if (dma->tx_extra_dummy_dma) { /* @@ -996,13 +1172,23 @@ void process_tx_dma_interrupt(struct spi_periph *periph) { spi_configure_dma(dma->dma, dma->tx_chan, (uint32_t)dma->spidr, (uint32_t)&(dma->tx_dummy_buf), len_remaining, trans->dss, FALSE); +#ifdef STM32F1 dma_set_read_from_memory(dma->dma, dma->tx_chan); dma_set_priority(dma->dma, dma->tx_chan, DMA_CCR_PL_MEDIUM); +#elif defined STM32F4 + dma_channel_select(dma->dma, dma->tx_chan, dma->tx_chan_sel); + dma_set_transfer_mode(dma->dma, dma->tx_chan, DMA_SxCR_DIR_MEM_TO_PERIPHERAL); + dma_set_priority(dma->dma, dma->tx_chan, DMA_SxCR_PL_MEDIUM); +#endif /* Enable DMA transfer complete interrupts. */ dma_enable_transfer_complete_interrupt(dma->dma, dma->tx_chan); /* Enable DMA channels */ +#ifdef STM32F1 dma_enable_channel(dma->dma, dma->tx_chan); +#elif defined STM32F4 + dma_enable_stream(dma->dma, dma->tx_chan); +#endif /* Enable SPI transfers via DMA */ spi_enable_tx_dma((uint32_t)periph->reg_addr); @@ -1015,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.c b/sw/airborne/arch/stm32/mcu_periph/uart_arch.c index fb73f70983..335a90ed50 100644 --- a/sw/airborne/arch/stm32/mcu_periph/uart_arch.c +++ b/sw/airborne/arch/stm32/mcu_periph/uart_arch.c @@ -41,11 +41,8 @@ void uart_periph_set_baudrate(struct uart_periph* p, uint32_t baud) { - /* Configure USART */ + /* Configure USART baudrate */ usart_set_baudrate((uint32_t)p->reg_addr, baud); - usart_set_databits((uint32_t)p->reg_addr, 8); - usart_set_stopbits((uint32_t)p->reg_addr, USART_STOPBITS_1); - usart_set_parity((uint32_t)p->reg_addr, USART_PARITY_NONE); /* Disable Idle Line interrupt */ USART_CR1((uint32_t)p->reg_addr) &= ~USART_CR1_IDLEIE; @@ -61,6 +58,33 @@ void uart_periph_set_baudrate(struct uart_periph* p, uint32_t baud) { } +void uart_periph_set_bits_stop_parity(struct uart_periph* p, uint8_t bits, uint8_t stop, uint8_t parity) { + /* Configure USART parity and data bits */ + if (parity == UPARITY_EVEN) { + usart_set_parity((uint32_t)p->reg_addr, USART_PARITY_EVEN); + if (bits == UBITS_7) + usart_set_databits((uint32_t)p->reg_addr, 8); + else // 8 data bits by default + usart_set_databits((uint32_t)p->reg_addr, 9); + } + else if (parity == USART_PARITY_ODD) { + usart_set_parity((uint32_t)p->reg_addr, USART_PARITY_ODD); + if (bits == UBITS_7) + usart_set_databits((uint32_t)p->reg_addr, 8); + else // 8 data bits by default + usart_set_databits((uint32_t)p->reg_addr, 9); + } + else { // 8 data bist, NO_PARITY by default + usart_set_parity((uint32_t)p->reg_addr, USART_PARITY_NONE); + usart_set_databits((uint32_t)p->reg_addr, 8); // is 7bits without parity possible ? + } + /* Configure USART stop bits */ + if (stop == USTOP_2) + usart_set_stopbits((uint32_t)p->reg_addr, USART_STOPBITS_2); + else // 1 stop bit by default + usart_set_stopbits((uint32_t)p->reg_addr, USART_STOPBITS_1); +} + void uart_periph_set_mode(struct uart_periph* p, bool_t tx_enabled, bool_t rx_enabled, bool_t hw_flow_control) { uint32_t mode = 0; if (tx_enabled) @@ -173,6 +197,18 @@ static inline void usart_enable_irq(uint8_t IRQn) { #define UART1_HW_FLOW_CONTROL FALSE #endif +#ifndef UART1_BITS +#define UART1_BITS UBITS_8 +#endif + +#ifndef UART1_STOP +#define UART1_STOP USTOP_1 +#endif + +#ifndef UART1_PARITY +#define UART1_PARITY UPARITY_NO +#endif + void uart1_init( void ) { uart_periph_init(&uart1); @@ -201,7 +237,8 @@ void uart1_init( void ) { /* Configure USART1, enable hardware flow control*/ uart_periph_set_mode(&uart1, USE_UART1_TX, USE_UART1_RX, UART1_HW_FLOW_CONTROL); - /* Set USART1 baudrate and enable interrupt */ + /* Set USART1 parameters and enable interrupt */ + uart_periph_set_bits_stop_parity(&uart1, UART1_BITS, UART1_STOP, UART1_PARITY); uart_periph_set_baudrate(&uart1, UART1_BAUD); } @@ -224,6 +261,18 @@ void usart1_isr(void) { usart_isr(&uart1); } #define UART2_HW_FLOW_CONTROL FALSE #endif +#ifndef UART2_BITS +#define UART2_BITS UBITS_8 +#endif + +#ifndef UART2_STOP +#define UART2_STOP USTOP_1 +#endif + +#ifndef UART2_PARITY +#define UART2_PARITY UPARITY_NO +#endif + void uart2_init( void ) { uart_periph_init(&uart2); @@ -253,6 +302,7 @@ void uart2_init( void ) { uart_periph_set_mode(&uart2, USE_UART2_TX, USE_UART2_RX, UART2_HW_FLOW_CONTROL); /* Configure USART */ + uart_periph_set_bits_stop_parity(&uart2, UART2_BITS, UART2_STOP, UART2_PARITY); uart_periph_set_baudrate(&uart2, UART2_BAUD); } @@ -275,6 +325,18 @@ void usart2_isr(void) { usart_isr(&uart2); } #define UART3_HW_FLOW_CONTROL FALSE #endif +#ifndef UART3_BITS +#define UART3_BITS UBITS_8 +#endif + +#ifndef UART3_STOP +#define UART3_STOP USTOP_1 +#endif + +#ifndef UART3_PARITY +#define UART3_PARITY UPARITY_NO +#endif + void uart3_init( void ) { uart_periph_init(&uart3); @@ -304,6 +366,7 @@ void uart3_init( void ) { uart_periph_set_mode(&uart3, USE_UART3_TX, USE_UART3_RX, UART3_HW_FLOW_CONTROL); /* Configure USART */ + uart_periph_set_bits_stop_parity(&uart3, UART3_BITS, UART3_STOP, UART3_PARITY); uart_periph_set_baudrate(&uart3, UART3_BAUD); } @@ -322,6 +385,18 @@ void usart3_isr(void) { usart_isr(&uart3); } #define USE_UART4_RX TRUE #endif +#ifndef UART4_BITS +#define UART4_BITS UBITS_8 +#endif + +#ifndef UART4_STOP +#define UART4_STOP USTOP_1 +#endif + +#ifndef UART4_PARITY +#define UART4_PARITY UPARITY_NO +#endif + void uart4_init( void ) { uart_periph_init(&uart4); @@ -342,6 +417,7 @@ void uart4_init( void ) { /* Configure USART */ uart_periph_set_mode(&uart4, USE_UART4_TX, USE_UART4_RX, FALSE); + uart_periph_set_bits_stop_parity(&uart4, UART4_BITS, UART4_STOP, UART4_PARITY); uart_periph_set_baudrate(&uart4, UART4_BAUD); } @@ -360,6 +436,18 @@ void uart4_isr(void) { usart_isr(&uart4); } #define USE_UART5_RX TRUE #endif +#ifndef UART5_BITS +#define UART5_BITS UBITS_8 +#endif + +#ifndef UART5_STOP +#define UART5_STOP USTOP_1 +#endif + +#ifndef UART5_PARITY +#define UART5_PARITY UPARITY_NO +#endif + void uart5_init( void ) { uart_periph_init(&uart5); @@ -380,6 +468,7 @@ void uart5_init( void ) { /* Configure USART */ uart_periph_set_mode(&uart5, USE_UART5_TX, USE_UART5_RX, FALSE); + uart_periph_set_bits_stop_parity(&uart5, UART5_BITS, UART5_STOP, UART5_PARITY); uart_periph_set_baudrate(&uart5, UART5_BAUD); } @@ -402,6 +491,18 @@ void uart5_isr(void) { usart_isr(&uart5); } #define UART6_HW_FLOW_CONTROL FALSE #endif +#ifndef UART6_BITS +#define UART6_BITS UBITS_8 +#endif + +#ifndef UART6_STOP +#define UART6_STOP USTOP_1 +#endif + +#ifndef UART6_PARITY +#define UART6_PARITY UPARITY_NO +#endif + void uart6_init( void ) { uart_periph_init(&uart6); @@ -430,7 +531,7 @@ void uart6_init( void ) { /* Configure USART Tx,Rx and hardware flow control*/ uart_periph_set_mode(&uart6, USE_UART6_TX, USE_UART6_RX, UART6_HW_FLOW_CONTROL); - + uart_periph_set_bits_stop_parity(&uart6, UART6_BITS, UART6_STOP, UART6_PARITY); uart_periph_set_baudrate(&uart6, UART6_BAUD); } 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 4a56067c5a..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 @@ -208,6 +272,15 @@ void actuators_pwm_arch_init(void) { #ifdef PWM_SERVO_8 set_servo_gpio(PWM_SERVO_8_GPIO, PWM_SERVO_8_PIN, PWM_SERVO_8_AF, PWM_SERVO_8_RCC_IOP); #endif +#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 @@ -230,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 @@ -262,5 +347,14 @@ void actuators_pwm_commit(void) { #ifdef PWM_SERVO_8 timer_set_oc_value(PWM_SERVO_8_TIMER, PWM_SERVO_8_OC, actuators_pwm_values[PWM_SERVO_8]); #endif +#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 new file mode 100644 index 0000000000..a13032bf7c --- /dev/null +++ b/sw/airborne/arch/stm32/subsystems/imu/imu_krooz_sd_arch.c @@ -0,0 +1,37 @@ +#include "subsystems/imu.h" + +#include +#include +#include +#include + +#include "subsystems/imu/imu_krooz_sd_arch.h" + +void imu_krooz_sd_arch_init(void) { + rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_SYSCFGEN); + rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_IOPBEN); + rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_IOPCEN); + gpio_mode_setup(GPIOB, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO5); + gpio_mode_setup(GPIOC, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO6); + + nvic_enable_irq(NVIC_EXTI9_5_IRQ); + exti_select_source(EXTI5, GPIOB); + exti_select_source(EXTI6, GPIOC); + exti_set_trigger(EXTI5, EXTI_TRIGGER_RISING); + exti_set_trigger(EXTI6, EXTI_TRIGGER_FALLING); + exti_enable_request(EXTI5); + exti_enable_request(EXTI6); + nvic_set_priority(NVIC_EXTI9_5_IRQ, 0x0F); +} + +void exti9_5_isr(void) { + /* clear EXTI */ + if(EXTI_PR & EXTI6) { + exti_reset_request(EXTI6); + imu_krooz.hmc_eoc = TRUE; + } + if(EXTI_PR & EXTI5) { + exti_reset_request(EXTI5); + imu_krooz.mpu_eoc = TRUE; + } +} diff --git a/sw/airborne/arch/stm32/subsystems/imu/imu_krooz_sd_arch.h b/sw/airborne/arch/stm32/subsystems/imu/imu_krooz_sd_arch.h new file mode 100644 index 0000000000..3917845d68 --- /dev/null +++ b/sw/airborne/arch/stm32/subsystems/imu/imu_krooz_sd_arch.h @@ -0,0 +1,8 @@ +#ifndef IMU_KROOZ_SD_ARCH_H +#define IMU_KROOZ_SD_ARCH_H + +#include "subsystems/imu.h" + +void imu_krooz_sd_arch_init(void); + +#endif /* IMU_KROOZ_SD_ARCH_H */ diff --git a/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.c b/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.c index b979a4e2d3..db20493eda 100644 --- a/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.c +++ b/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.c @@ -52,6 +52,20 @@ uint32_t ppm_last_pulse_time; bool_t ppm_data_valid; static uint32_t timer_rollover_cnt; +/* + * Timer clock frequency (before prescaling) is twice the frequency + * of the APB domain to which the timer is connected. + */ + +#ifdef STM32F1 +/** + * HCLK = 72MHz, Timer clock also 72MHz since + * TIM1_CLK = APB2 = 72MHz + * TIM2_CLK = 2 * APB1 = 2 * 32MHz + */ +#define PPM_TIMER_CLK AHB_CLK +#endif + #if USE_PPM_TIM2 PRINT_CONFIG_MSG("Using TIM2 for PPM input.") @@ -60,6 +74,14 @@ PRINT_CONFIG_MSG("Using TIM2 for PPM input.") #define PPM_PERIPHERAL RCC_APB1ENR_TIM2EN #define PPM_TIMER TIM2 +#ifdef STM32F4 +/* Since APB prescaler != 1 : + * Timer clock frequency (before prescaling) is twice the frequency + * of the APB domain to which the timer is connected. + */ +#define PPM_TIMER_CLK (rcc_ppre1_frequency * 2) +#endif + #elif USE_PPM_TIM1 PRINT_CONFIG_MSG("Using TIM1 for PPM input.") @@ -68,6 +90,12 @@ PRINT_CONFIG_MSG("Using TIM1 for PPM input.") #define PPM_PERIPHERAL RCC_APB2ENR_TIM1EN #define PPM_TIMER TIM1 +#ifdef STM32F4 +#define PPM_TIMER_CLK (rcc_ppre2_frequency * 2) +#endif + +#else +#error Unknown PPM input timer configuration. #endif void ppm_arch_init ( void ) { @@ -86,7 +114,7 @@ void ppm_arch_init ( void ) { timer_set_mode(PPM_TIMER, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP); timer_set_period(PPM_TIMER, 0xFFFF); - timer_set_prescaler(PPM_TIMER, (AHB_CLK / (RC_PPM_TICKS_PER_USEC*ONE_MHZ_CLK)) - 1); + timer_set_prescaler(PPM_TIMER, (PPM_TIMER_CLK / (RC_PPM_TICKS_PER_USEC*ONE_MHZ_CLK)) - 1); /* TIM configuration: Input Capture mode --------------------- The Rising edge is used as active edge @@ -96,7 +124,7 @@ void ppm_arch_init ( void ) { #elif defined PPM_PULSE_TYPE && PPM_PULSE_TYPE == PPM_PULSE_TYPE_NEGATIVE timer_ic_set_polarity(PPM_TIMER, PPM_CHANNEL, TIM_IC_FALLING); #else -#error "Unknown PM_PULSE_TYPE" +#error "Unknown PPM_PULSE_TYPE" #endif timer_ic_set_input(PPM_TIMER, PPM_CHANNEL, PPM_TIMER_INPUT); timer_ic_set_prescaler(PPM_TIMER, PPM_CHANNEL, TIM_IC_PSC_OFF); diff --git a/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.h b/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.h index fbe1ece8da..4d866004f1 100644 --- a/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.h +++ b/sw/airborne/arch/stm32/subsystems/radio_control/ppm_arch.h @@ -33,10 +33,17 @@ #include "mcu_periph/sys_time.h" /** - * The ppm counter is running at cpu freq / 72 or 168 / 8 - * so the counter has 1/8 us resolution + * The ppm counter is set-up to have 1/6 us resolution. + * + * The timer clock frequency (before prescaling): + * STM32F1: + * TIM1 -> APB2 = HCLK = 72MHz + * TIM2 -> 2 * APB1 = 2 * 36MHz = 72MHz + * STM32F4: + * TIM1 -> 2 * APB2 = 2 * 84MHz = 168MHz + * TIM2 -> 2 * APB1 = 2 * 42MHz = 84MHz */ -#define RC_PPM_TICKS_PER_USEC 8 +#define RC_PPM_TICKS_PER_USEC 6 #define RC_PPM_TICKS_OF_USEC(_v) ((_v)*RC_PPM_TICKS_PER_USEC) #define RC_PPM_SIGNED_TICKS_OF_USEC(_v) (int32_t)((_v)*RC_PPM_TICKS_PER_USEC) diff --git a/sw/airborne/arch/stm32/subsystems/settings_arch.c b/sw/airborne/arch/stm32/subsystems/settings_arch.c index 411e97552d..3b814a5df3 100644 --- a/sw/airborne/arch/stm32/subsystems/settings_arch.c +++ b/sw/airborne/arch/stm32/subsystems/settings_arch.c @@ -35,12 +35,7 @@ #include "subsystems/settings.h" -#if defined(STM32F1) -#include -#elif defined(STM32F4) -#include -#endif - +#include #include #include diff --git a/sw/airborne/boards/apogee_0.99.h b/sw/airborne/boards/apogee_0.99.h index b73511266b..8a79bdc24b 100644 --- a/sw/airborne/boards/apogee_0.99.h +++ b/sw/airborne/boards/apogee_0.99.h @@ -60,6 +60,18 @@ #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 + /* Onboard ADCs */ #define USE_AD_TIM4 1 diff --git a/sw/airborne/boards/apogee_1.0.h b/sw/airborne/boards/apogee_1.0.h new file mode 100644 index 0000000000..343c514eca --- /dev/null +++ b/sw/airborne/boards/apogee_1.0.h @@ -0,0 +1,399 @@ +#ifndef CONFIG_APOGEE_1_00_H +#define CONFIG_APOGEE_1_00_H + +#define BOARD_APOGEE + +/* Apogee has a 16MHz external clock and 168MHz internal. */ +#define EXT_CLK 16000000 +#define AHB_CLK 168000000 + +/* + * Onboard LEDs + */ + +/* red, on PC0 */ +#ifndef USE_LED_1 +#define USE_LED_1 1 +#endif +#define LED_1_GPIO GPIOC +#define LED_1_GPIO_CLK RCC_AHB1ENR_IOPCEN +#define LED_1_GPIO_PIN GPIO0 +#define LED_1_GPIO_ON gpio_clear +#define LED_1_GPIO_OFF gpio_set +#define LED_1_AFIO_REMAP ((void)0) + +/* orange, on PC13 */ +#ifndef USE_LED_2 +#define USE_LED_2 1 +#endif +#define LED_2_GPIO GPIOC +#define LED_2_GPIO_CLK RCC_AHB1ENR_IOPCEN +#define LED_2_GPIO_PIN GPIO13 +#define LED_2_GPIO_ON gpio_clear +#define LED_2_GPIO_OFF gpio_set +#define LED_2_AFIO_REMAP ((void)0) + +/* green, on PC1 */ +#ifndef USE_LED_3 +#define USE_LED_3 1 +#endif +#define LED_3_GPIO GPIOC +#define LED_3_GPIO_CLK RCC_AHB1ENR_IOPCEN +#define LED_3_GPIO_PIN GPIO1 +#define LED_3_GPIO_ON gpio_clear +#define LED_3_GPIO_OFF gpio_set +#define LED_3_AFIO_REMAP ((void)0) + +/* yellow, on PC3 */ +#ifndef USE_LED_4 +#define USE_LED_4 1 +#endif +#define LED_4_GPIO GPIOC +#define LED_4_GPIO_CLK RCC_AHB1ENR_IOPCEN +#define LED_4_GPIO_PIN GPIO3 +#define LED_4_GPIO_ON gpio_clear +#define LED_4_GPIO_OFF gpio_set +#define LED_4_AFIO_REMAP ((void)0) + +/* AUX1, on PB1, 1 on LED_ON, 0 on LED_OFF */ +#ifndef USE_LED_5 +#define USE_LED_5 0 +#endif +#define LED_5_GPIO GPIOB +#define LED_5_GPIO_CLK RCC_AHB1ENR_IOPBEN +#define LED_5_GPIO_PIN GPIO1 +#define LED_5_GPIO_ON gpio_set +#define LED_5_GPIO_OFF gpio_clear +#define LED_5_AFIO_REMAP ((void)0) + +/* AUX2, on PC5, 1 on LED_ON, 0 on LED_OFF */ +#ifndef USE_LED_6 +#define USE_LED_6 0 +#endif +#define LED_6_GPIO GPIOC +#define LED_6_GPIO_CLK RCC_AHB1ENR_IOPCEN +#define LED_6_GPIO_PIN GPIO5 +#define LED_6_GPIO_ON gpio_set +#define LED_6_GPIO_OFF gpio_clear +#define LED_6_AFIO_REMAP ((void)0) + +/* AUX3, on PC4, 1 on LED_ON, 0 on LED_OFF */ +#ifndef USE_LED_7 +#define USE_LED_7 0 +#endif +#define LED_7_GPIO GPIOC +#define LED_7_GPIO_CLK RCC_AHB1ENR_IOPCEN +#define LED_7_GPIO_PIN GPIO4 +#define LED_7_GPIO_ON gpio_set +#define LED_7_GPIO_OFF gpio_clear +#define LED_7_AFIO_REMAP ((void)0) + +/* AUX4, on PB15, 1 on LED_ON, 0 on LED_OFF */ +#ifndef USE_LED_8 +#define USE_LED_8 0 +#endif +#define LED_8_GPIO GPIOB +#define LED_8_GPIO_CLK RCC_AHB1ENR_IOPBEN +#define LED_8_GPIO_PIN GPIO15 +#define LED_8_GPIO_ON gpio_set +#define LED_8_GPIO_OFF gpio_clear +#define LED_8_AFIO_REMAP ((void)0) + +/* Power Switch, on PB12, 1 on LED_ON, 0 on LED_OFF */ +#ifndef USE_LED_9 +#define USE_LED_9 1 +#endif +#define LED_9_GPIO GPIOB +#define LED_9_GPIO_CLK RCC_AHB1ENR_IOPBEN +#define LED_9_GPIO_PIN GPIO12 +#define LED_9_GPIO_ON gpio_set +#define LED_9_GPIO_OFF gpio_clear +#define LED_9_AFIO_REMAP ((void)0) + +#define POWER_SWITCH_LED 9 + + +/* Pint to set Uart2 RX polarity, on PB13, output high inverts, low doesn't */ +#define RC_POLARITY_GPIO_PORT GPIOB +#define RC_POLARITY_GPIO_PIN GPIO13 + + +/* 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.006185*adc) + +/* 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 USE_UART2_TX FALSE + +#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 + + +/* 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 + + +/* I2C mapping */ +#define I2C1_GPIO_PORT GPIOB +#define I2C1_GPIO_SCL GPIO8 +#define I2C1_GPIO_SDA GPIO7 + +#define I2C2_GPIO_PORT GPIOB +#define I2C2_GPIO_SCL GPIO10 +#define I2C2_GPIO_SDA GPIO11 + + +/* Activate onboard baro */ +#define BOARD_HAS_BARO 1 + +/* PWM */ +#define PWM_USE_TIM2 1 +#define PWM_USE_TIM3 1 + +#define USE_PWM0 1 +#define USE_PWM1 1 +#define USE_PWM2 1 +#define USE_PWM3 1 +#define USE_PWM4 1 +#define USE_PWM5 1 + +// PWM_SERVO_x is the index of the servo in the actuators_pwm_values array +#if USE_PWM0 +#define PWM_SERVO_0 0 +#define PWM_SERVO_0_TIMER TIM3 +#define PWM_SERVO_0_RCC_IOP RCC_AHB1ENR_IOPBEN +#define PWM_SERVO_0_GPIO GPIOB +#define PWM_SERVO_0_PIN GPIO0 +#define PWM_SERVO_0_AF GPIO_AF2 +#define PWM_SERVO_0_OC TIM_OC3 +#define PWM_SERVO_0_OC_BIT (1<<2) +#else +#define PWM_SERVO_0_OC_BIT 0 +#endif + +#if USE_PWM1 +#define PWM_SERVO_1 1 +#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 GPIO2 +#define PWM_SERVO_1_AF GPIO_AF1 +#define PWM_SERVO_1_OC TIM_OC3 +#define PWM_SERVO_1_OC_BIT (1<<2) +#else +#define PWM_SERVO_1_OC_BIT 0 +#endif + +#if USE_PWM2 +#define PWM_SERVO_2 2 +#define PWM_SERVO_2_TIMER TIM3 +#define PWM_SERVO_2_RCC_IOP RCC_AHB1ENR_IOPBEN +#define PWM_SERVO_2_GPIO GPIOB +#define PWM_SERVO_2_PIN GPIO5 +#define PWM_SERVO_2_AF GPIO_AF2 +#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 3 +#define PWM_SERVO_3_TIMER TIM3 +#define PWM_SERVO_3_RCC_IOP RCC_AHB1ENR_IOPBEN +#define PWM_SERVO_3_GPIO GPIOB +#define PWM_SERVO_3_PIN GPIO4 +#define PWM_SERVO_3_AF GPIO_AF2 +#define PWM_SERVO_3_OC TIM_OC1 +#define PWM_SERVO_3_OC_BIT (1<<0) +#else +#define PWM_SERVO_3_OC_BIT 0 +#endif + +#if USE_PWM4 +#define PWM_SERVO_4 4 +#define PWM_SERVO_4_TIMER TIM2 +#define PWM_SERVO_4_RCC_IOP RCC_AHB1ENR_IOPBEN +#define PWM_SERVO_4_GPIO GPIOB +#define PWM_SERVO_4_PIN GPIO3 +#define PWM_SERVO_4_AF GPIO_AF1 +#define PWM_SERVO_4_OC TIM_OC2 +#define PWM_SERVO_4_OC_BIT (1<<1) +#else +#define PWM_SERVO_4_OC_BIT 0 +#endif + +#if USE_PWM5 +#define PWM_SERVO_5 5 +#define PWM_SERVO_5_TIMER TIM2 +#define PWM_SERVO_5_RCC_IOP RCC_AHB1ENR_IOPAEN +#define PWM_SERVO_5_GPIO GPIOA +#define PWM_SERVO_5_PIN GPIO15 +#define PWM_SERVO_5_AF GPIO_AF1 +#define PWM_SERVO_5_OC TIM_OC1 +#define PWM_SERVO_5_OC_BIT (1<<0) +#else +#define PWM_SERVO_5_OC_BIT 0 +#endif + +// PWM AUX1 (conflict with ADC0) +#if USE_PWM6 +#define PWM_SERVO_6 6 +#define PWM_SERVO_6_TIMER TIM3 +#define PWM_SERVO_6_RCC_IOP RCC_AHB1ENR_IOPBEN +#define PWM_SERVO_6_GPIO GPIOB +#define PWM_SERVO_6_PIN GPIO1 +#define PWM_SERVO_6_AF GPIO_AF2 +#define PWM_SERVO_6_OC TIM_OC4 +#define PWM_SERVO_6_OC_BIT (1<<3) +#else +#define PWM_SERVO_6_OC_BIT 0 +#endif + + +#define PWM_TIM2_CHAN_MASK (PWM_SERVO_1_OC_BIT|PWM_SERVO_4_OC_BIT|PWM_SERVO_5_OC_BIT) +#define PWM_TIM3_CHAN_MASK (PWM_SERVO_0_OC_BIT|PWM_SERVO_2_OC_BIT|PWM_SERVO_3_OC_BIT|PWM_SERVO_6_OC_BIT) + +/* + * 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_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 + +#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 431030eb31..79084315ef 100644 --- a/sw/airborne/boards/ardrone/at_com.h +++ b/sw/airborne/boards/ardrone/at_com.h @@ -178,7 +178,7 @@ typedef struct _navdata_gps_t { uint8_t unk_2[16]; struct{ uint8_t sat; - uint8_t unk; + uint8_t cn0; }channels[12]; int32_t gps_plugged; /*!< When the gps is plugged */ uint8_t unk_3[108]; @@ -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 8231957d70..e4f43a136a 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,21 +34,81 @@ #include #include #include +#include +#include + +#include "std.h" #include "navdata.h" +#include "subsystems/ins.h" -int nav_fd; +#define NAVDATA_PACKET_SIZE 60 +#define NAVDATA_START_BYTE 0x3a -int navdata_init() +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) { - port = malloc(sizeof(navdata_port)); + size_t written = 0; - nav_fd = open("/dev/ttyO1", O_RDWR | O_NOCTTY | O_NONBLOCK); - if (nav_fd == -1) + while(written < count) { - perror("navdata_init: Unable to open /dev/ttyO1 - "); - return 1; - } else { - port->isOpen = 1; + 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"); +} + +bool_t navdata_init() +{ + if (nav_fd <= 0) { + nav_fd = open("/dev/ttyO1", O_RDWR | O_NOCTTY | O_NONBLOCK); + + 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 @@ -70,149 +130,246 @@ 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; - 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.h b/sw/airborne/boards/krooz/baro_board.h new file mode 100644 index 0000000000..5f47ad650e --- /dev/null +++ b/sw/airborne/boards/krooz/baro_board.h @@ -0,0 +1,14 @@ + +/* + * 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 + +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 new file mode 100644 index 0000000000..8d369be25a --- /dev/null +++ b/sw/airborne/boards/krooz/imu_krooz.c @@ -0,0 +1,188 @@ +/* + * Copyright (C) 2013 Sergey Krukowski + * + * 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/krooz/imu_krooz.c + * + * Driver for the IMU on the KroozSD board. + * + * Invensense MPU-6050 + * Honeywell HMC-5883 + */ + +#include +#include "boards/krooz/imu_krooz.h" +#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" +#include "messages.h" +#include "subsystems/datalink/downlink.h" + +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#endif + +#if !defined KROOZ_LOWPASS_FILTER && !defined KROOZ_SMPLRT_DIV +#define KROOZ_LOWPASS_FILTER MPU60X0_DLPF_256HZ +#define KROOZ_SMPLRT_DIV 1 +#endif +PRINT_CONFIG_VAR(KROOZ_SMPLRT_DIV) +PRINT_CONFIG_VAR(KROOZ_LOWPASS_FILTER) + +#ifndef KROOZ_GYRO_RANGE +#define KROOZ_GYRO_RANGE MPU60X0_GYRO_RANGE_250 +#endif +PRINT_CONFIG_VAR(KROOZ_GYRO_RANGE) + +#ifndef KROOZ_ACCEL_RANGE +#define KROOZ_ACCEL_RANGE MPU60X0_ACCEL_RANGE_2G +#endif +PRINT_CONFIG_VAR(KROOZ_ACCEL_RANGE) + +struct ImuKrooz imu_krooz; + + +#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 ) +{ + ///////////////////////////////////////////////////////////////////// + // MPU-60X0 + mpu60x0_i2c_init(&imu_krooz.mpu, &(IMU_KROOZ_I2C_DEV), MPU60X0_ADDR); + // change the default configuration + imu_krooz.mpu.config.smplrt_div = KROOZ_SMPLRT_DIV; + imu_krooz.mpu.config.dlpf_cfg = KROOZ_LOWPASS_FILTER; + imu_krooz.mpu.config.gyro_range = KROOZ_GYRO_RANGE; + imu_krooz.mpu.config.accel_range = KROOZ_ACCEL_RANGE; + imu_krooz.mpu.config.drdy_int_enable = TRUE; + + hmc58xx_init(&imu_krooz.hmc, &(IMU_KROOZ_I2C_DEV), HMC58XX_ADDR); + + // Init median filters +#if IMU_KROOZ_USE_GYRO_MEDIAN_FILTER + InitMedianFilterRatesInt(median_gyro); +#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); + imu_krooz.meas_nb = 0; + + imu_krooz.gyr_valid = FALSE; + 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(); +} + +void imu_periodic( void ) +{ + // Start reading the latest gyroscope data + if (!imu_krooz.mpu.config.initialized) + mpu60x0_i2c_start_configure(&imu_krooz.mpu); + + if (!imu_krooz.hmc.initialized) + 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 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 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; + + imu_krooz.gyr_valid = TRUE; + imu_krooz.acc_valid = TRUE; + } + + //RunOnceEvery(10,imu_krooz_downlink_raw()); +} + +void imu_krooz_downlink_raw( void ) +{ + DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice,&imu.gyro_unscaled.p,&imu.gyro_unscaled.q,&imu.gyro_unscaled.r); + DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice,&imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z); +} + + +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) { + RATES_ADD(imu_krooz.rates_sum, imu_krooz.mpu.data_rates.rates); + VECT3_ADD(imu_krooz.accel_sum, imu_krooz.mpu.data_accel.vect); + imu_krooz.meas_nb++; + 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_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); + 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 new file mode 100644 index 0000000000..565e48a838 --- /dev/null +++ b/sw/airborne/boards/krooz/imu_krooz.h @@ -0,0 +1,153 @@ +/* + * Copyright (C) 2013 Sergey Krukowski + * + * 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/krooz/imu_krooz.h + * + * Driver for the IMU on the KroozSD board. + * + * Invensense MPU-6050 + */ + +#ifndef IMU_KROOZ_H +#define IMU_KROOZ_H + +#include "std.h" +#include "generated/airframe.h" +#include "subsystems/imu.h" + +#include "peripherals/mpu60x0_i2c.h" +#include "peripherals/hmc58xx.h" + +// 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_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 + +/** default gyro sensitivy and neutral from the datasheet + * MPU with 250 deg/s has 131.072 LSB/(deg/s) + * sens = 1/131.072 * pi/180 * 2^INT32_RATE_FRAC + * sens = 1/131.072 * pi/180 * 4096 = 0.5454 + I*/ +#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS +// FIXME +#define IMU_GYRO_P_SENS 0.5454 +#define IMU_GYRO_P_SENS_NUM 2727 +#define IMU_GYRO_P_SENS_DEN 5000 +#define IMU_GYRO_Q_SENS 0.5454 +#define IMU_GYRO_Q_SENS_NUM 2727 +#define IMU_GYRO_Q_SENS_DEN 5000 +#define IMU_GYRO_R_SENS 0.5454 +#define IMU_GYRO_R_SENS_NUM 2727 +#define IMU_GYRO_R_SENS_DEN 5000 +#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 + * MPU with 2g has 16384 LSB/g + * sens = 9.81 [m/s^2] / 16384 [LSB/g] * 2^INT32_ACCEL_FRAC = 0.6131 + */ +#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS +// FIXME +#define IMU_ACCEL_X_SENS 0.6131 +#define IMU_ACCEL_X_SENS_NUM 6131 +#define IMU_ACCEL_X_SENS_DEN 10000 +#define IMU_ACCEL_Y_SENS 0.6131 +#define IMU_ACCEL_Y_SENS_NUM 6131 +#define IMU_ACCEL_Y_SENS_DEN 10000 +#define IMU_ACCEL_Z_SENS 0.6131 +#define IMU_ACCEL_Z_SENS_NUM 6131 +#define IMU_ACCEL_Z_SENS_DEN 10000 +#endif +#if !defined IMU_ACCEL_X_NEUTRAL & !defined IMU_ACCEL_Y_NEUTRAL & !defined IMU_ACCEL_Z_NEUTRAL +#define IMU_ACCEL_X_NEUTRAL 0 +#define IMU_ACCEL_Y_NEUTRAL 0 +#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; + + +/* must be defined in order to be IMU code: declared in imu.h +extern void imu_impl_init(void); +extern void imu_periodic(void); +*/ + +/* Own Extra Functions */ +extern void imu_krooz_event( void ); +extern void imu_krooz_downlink_raw( void ); + +static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void) __attribute__((unused))) { + imu_krooz_event(); + if (imu_krooz.gyr_valid) { + imu_krooz.gyr_valid = FALSE; + _gyro_handler(); + } + if (imu_krooz.acc_valid) { + imu_krooz.acc_valid = FALSE; + _accel_handler(); + } + if (imu_krooz.mag_valid) { + imu_krooz.mag_valid = FALSE; + _mag_handler(); + } +} + +#endif // IMU_KROOZ_H diff --git a/sw/airborne/boards/krooz_sd.h b/sw/airborne/boards/krooz_sd.h index 6e6899bbce..1ecd270753 100644 --- a/sw/airborne/boards/krooz_sd.h +++ b/sw/airborne/boards/krooz_sd.h @@ -1,9 +1,9 @@ -#ifndef CONFIG_KROOZ_1_0_H -#define CONFIG_KROOZ_1_0_H +#ifndef CONFIG_KROOZ_SD_H +#define CONFIG_KROOZ_SD_H #define BOARD_KROOZ -/* Krooz/M has a 12MHz external clock and 168MHz internal. */ +/* KroozSD has a 12MHz external clock and 168MHz internal. */ #define EXT_CLK 12000000 #define AHB_CLK 168000000 @@ -99,16 +99,43 @@ #define UART5_GPIO_PORT_TX GPIOC #define UART5_GPIO_TX GPIO12 +/* 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 SPI2_GPIO_AF GPIO_AF5 +#define SPI2_GPIO_PORT_MISO GPIOB +#define SPI2_GPIO_MISO GPIO14 +#define SPI2_GPIO_PORT_MOSI GPIOB +#define SPI2_GPIO_MOSI GPIO15 +#define SPI2_GPIO_PORT_SCK GPIOB +#define SPI2_GPIO_SCK GPIO13 + +#define SPI_SELECT_SLAVE0_PORT GPIOA +#define SPI_SELECT_SLAVE0_PIN GPIO4 +#define SPI_SELECT_SLAVE1_PORT GPIOB +#define SPI_SELECT_SLAVE1_PIN GPIO12 +#define SPI_SELECT_SLAVE2_PORT GPIOB +#define SPI_SELECT_SLAVE2_PIN GPIO2 /* 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 #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 /* Onboard ADCs */ #define USE_AD_TIM1 1 @@ -197,20 +224,10 @@ } #endif // USE_AD1 - -/* I2C mapping */ -#define GPIO_I2C1_SCL GPIO8 -#define GPIO_I2C1_SDA GPIO9 -#define GPIO_I2C2_SCL GPIO10 -#define GPIO_I2C2_SDA GPIO11 -#define GPIO_I2C3_SCL GPIO8 //PA8 -#define GPIO_I2C3_SDA GPIO9 //PC9 - /* Activate onboard baro */ #define BOARD_HAS_BARO 1 /* PWM */ -#define PWM_USE_TIM2 1 #define PWM_USE_TIM3 1 #define PWM_USE_TIM4 1 #define PWM_USE_TIM5 1 @@ -227,18 +244,23 @@ #define USE_PWM9 1 //#define USE_PWM10 1 +#if USE_PWM10 +#define ACTUATORS_PWM_NB 11 +#define PWM_USE_TIM2 1 +#else #define ACTUATORS_PWM_NB 10 +#endif // PWM_SERVO_x is the index of the servo in the actuators_pwm_values array #if USE_PWM0 #define PWM_SERVO_0 0 #define PWM_SERVO_0_TIMER TIM3 -#define PWM_SERVO_0_RCC_IOP RCC_AHB1ENR_IOPCEN -#define PWM_SERVO_0_GPIO GPIOC -#define PWM_SERVO_0_PIN GPIO6 -#define PWM_SERVO_0_AF GPIO_AF1 -#define PWM_SERVO_0_OC TIM_OC1 -#define PWM_SERVO_0_OC_BIT (1<<0) +#define PWM_SERVO_0_RCC_IOP RCC_AHB1ENR_IOPBEN +#define PWM_SERVO_0_GPIO GPIOB +#define PWM_SERVO_0_PIN GPIO1 +#define PWM_SERVO_0_AF GPIO_AF2 +#define PWM_SERVO_0_OC TIM_OC4 +#define PWM_SERVO_0_OC_BIT (1<<3) #else #define PWM_SERVO_0_OC_BIT 0 #endif @@ -248,10 +270,10 @@ #define PWM_SERVO_1_TIMER TIM3 #define PWM_SERVO_1_RCC_IOP RCC_AHB1ENR_IOPCEN #define PWM_SERVO_1_GPIO GPIOC -#define PWM_SERVO_1_PIN GPIO7 -#define PWM_SERVO_1_AF GPIO_AF1 -#define PWM_SERVO_1_OC TIM_OC2 -#define PWM_SERVO_1_OC_BIT (1<<1) +#define PWM_SERVO_1_PIN GPIO8 +#define PWM_SERVO_1_AF GPIO_AF2 +#define PWM_SERVO_1_OC TIM_OC3 +#define PWM_SERVO_1_OC_BIT (1<<2) #else #define PWM_SERVO_1_OC_BIT 0 #endif @@ -261,23 +283,23 @@ #define PWM_SERVO_2_TIMER TIM3 #define PWM_SERVO_2_RCC_IOP RCC_AHB1ENR_IOPCEN #define PWM_SERVO_2_GPIO GPIOC -#define PWM_SERVO_2_PIN GPIO8 -#define PWM_SERVO_2_AF GPIO_AF1 -#define PWM_SERVO_2_OC TIM_OC3 -#define PWM_SERVO_2_OC_BIT (1<<2) +#define PWM_SERVO_2_PIN GPIO7 +#define PWM_SERVO_2_AF GPIO_AF2 +#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_IDX 3 +#define PWM_SERVO_3 3 #define PWM_SERVO_3_TIMER TIM3 -#define PWM_SERVO_3_RCC_IOP RCC_AHB1ENR_IOPCEN -#define PWM_SERVO_3_GPIO GPIOC -#define PWM_SERVO_3_PIN GPIO9 -#define PWM_SERVO_3_AF GPIO_AF1 -#define PWM_SERVO_3_OC TIM_OC4 -#define PWM_SERVO_3_OC_BIT (1<<3) +#define PWM_SERVO_3_RCC_IOP RCC_AHB1ENR_IOPBEN +#define PWM_SERVO_3_GPIO GPIOB +#define PWM_SERVO_3_PIN GPIO4 +#define PWM_SERVO_3_AF GPIO_AF2 +#define PWM_SERVO_3_OC TIM_OC1 +#define PWM_SERVO_3_OC_BIT (1<<0) #else #define PWM_SERVO_3_OC_BIT 0 #endif @@ -287,10 +309,10 @@ #define PWM_SERVO_4_TIMER TIM4 #define PWM_SERVO_4_RCC_IOP RCC_AHB1ENR_IOPBEN #define PWM_SERVO_4_GPIO GPIOB -#define PWM_SERVO_4_PIN GPIO6 -#define PWM_SERVO_4_AF GPIO_AF1 -#define PWM_SERVO_4_OC TIM_OC1 -#define PWM_SERVO_4_OC_BIT (1<<0) +#define PWM_SERVO_4_PIN GPIO7 +#define PWM_SERVO_4_AF GPIO_AF2 +#define PWM_SERVO_4_OC TIM_OC2 +#define PWM_SERVO_4_OC_BIT (1<<1) #else #define PWM_SERVO_4_OC_BIT 0 #endif @@ -300,10 +322,10 @@ #define PWM_SERVO_5_TIMER TIM4 #define PWM_SERVO_5_RCC_IOP RCC_AHB1ENR_IOPBEN #define PWM_SERVO_5_GPIO GPIOB -#define PWM_SERVO_5_PIN GPIO7 -#define PWM_SERVO_5_AF GPIO_AF1 -#define PWM_SERVO_5_OC TIM_OC2 -#define PWM_SERVO_5_OC_BIT (1<<1) +#define PWM_SERVO_5_PIN GPIO6 +#define PWM_SERVO_5_AF GPIO_AF2 +#define PWM_SERVO_5_OC TIM_OC1 +#define PWM_SERVO_5_OC_BIT (1<<0) #else #define PWM_SERVO_5_OC_BIT 0 #endif @@ -313,10 +335,10 @@ #define PWM_SERVO_6_TIMER TIM5 #define PWM_SERVO_6_RCC_IOP RCC_AHB1ENR_IOPAEN #define PWM_SERVO_6_GPIO GPIOA -#define PWM_SERVO_6_PIN GPIO0 -#define PWM_SERVO_6_AF GPIO_AF1 -#define PWM_SERVO_6_OC TIM_OC1 -#define PWM_SERVO_6_OC_BIT (1<<0) +#define PWM_SERVO_6_PIN GPIO3 +#define PWM_SERVO_6_AF GPIO_AF2 +#define PWM_SERVO_6_OC TIM_OC4 +#define PWM_SERVO_6_OC_BIT (1<<3) #else #define PWM_SERVO_6_OC_BIT 0 #endif @@ -326,10 +348,10 @@ #define PWM_SERVO_7_TIMER TIM5 #define PWM_SERVO_7_RCC_IOP RCC_AHB1ENR_IOPAEN #define PWM_SERVO_7_GPIO GPIOA -#define PWM_SERVO_7_PIN GPIO1 -#define PWM_SERVO_7_AF GPIO_AF1 -#define PWM_SERVO_7_OC TIM_OC2 -#define PWM_SERVO_7_OC_BIT (1<<1) +#define PWM_SERVO_7_PIN GPIO2 +#define PWM_SERVO_7_AF GPIO_AF2 +#define PWM_SERVO_7_OC TIM_OC3 +#define PWM_SERVO_7_OC_BIT (1<<2) #else #define PWM_SERVO_7_OC_BIT 0 #endif @@ -339,10 +361,10 @@ #define PWM_SERVO_8_TIMER TIM5 #define PWM_SERVO_8_RCC_IOP RCC_AHB1ENR_IOPAEN #define PWM_SERVO_8_GPIO GPIOA -#define PWM_SERVO_8_PIN GPIO2 -#define PWM_SERVO_8_AF GPIO_AF1 -#define PWM_SERVO_8_OC TIM_OC3 -#define PWM_SERVO_8_OC_BIT (1<<2) +#define PWM_SERVO_8_PIN GPIO1 +#define PWM_SERVO_8_AF GPIO_AF2 +#define PWM_SERVO_8_OC TIM_OC2 +#define PWM_SERVO_8_OC_BIT (1<<1) #else #define PWM_SERVO_8_OC_BIT 0 #endif @@ -352,10 +374,10 @@ #define PWM_SERVO_9_TIMER TIM5 #define PWM_SERVO_9_RCC_IOP RCC_AHB1ENR_IOPAEN #define PWM_SERVO_9_GPIO GPIOA -#define PWM_SERVO_9_PIN GPIO3 -#define PWM_SERVO_9_AF GPIO_AF1 -#define PWM_SERVO_9_OC TIM_OC4 -#define PWM_SERVO_9_OC_BIT (1<<3) +#define PWM_SERVO_9_PIN GPIO0 +#define PWM_SERVO_9_AF GPIO_AF2 +#define PWM_SERVO_9_OC TIM_OC1 +#define PWM_SERVO_9_OC_BIT (1<<0) #else #define PWM_SERVO_9_OC_BIT 0 #endif @@ -383,11 +405,11 @@ #define USE_PPM_TIM2 1 #define PPM_CHANNEL TIM_IC2 -#define PPM_TIMER_INPUT TIM_IC_IN_TI1 +#define PPM_TIMER_INPUT TIM_IC_IN_TI2 #define PPM_IRQ NVIC_TIM2_IRQ //#define PPM_IRQ2 NVIC_TIM2_UP_TIM10_IRQ // Capture/Compare InteruptEnable and InterruptFlag -#define PPM_CC_EN TIM_DIER_CC2IE +#define PPM_CC_IE TIM_DIER_CC2IE #define PPM_CC_IF TIM_SR_CC2IF #define PPM_GPIO_PORT GPIOB #define PPM_GPIO_PIN GPIO3 @@ -397,7 +419,7 @@ * Spektrum */ /* The line that is pulled low at power up to initiate the bind process */ -#define SPEKTRUM_BIND_PIN GPIO8 +#define SPEKTRUM_BIND_PIN GPIO9 #define SPEKTRUM_BIND_PIN_PORT GPIOA -#endif /* CONFIG_KROOZ_1_0_H */ +#endif /* CONFIG_KROOZ_SD_H */ 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 10994d5948..0000000000 --- a/sw/airborne/boards/lisa_m/baro_ms5611_i2c.c +++ /dev/null @@ -1,206 +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 -#ifndef DOWNLINK_DEVICE -#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE -#endif -#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 bd437cd318..0000000000 --- a/sw/airborne/boards/lisa_m/baro_ms5611_spi.c +++ /dev/null @@ -1,226 +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 -#ifndef DOWNLINK_DEVICE -#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE -#endif -#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/boards/umarim/imu_umarim.c b/sw/airborne/boards/umarim/imu_umarim.c index 248b178977..e70c5b24c0 100644 --- a/sw/airborne/boards/umarim/imu_umarim.c +++ b/sw/airborne/boards/umarim/imu_umarim.c @@ -32,7 +32,7 @@ #include #include "imu_umarim.h" #include "mcu_periph/i2c.h" -#include "led.h" +#include "generated/airframe.h" // Downlink #include "mcu_periph/uart.h" @@ -43,6 +43,11 @@ #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE #endif +#ifndef UMARIM_ACCEL_RANGE +#define UMARIM_ACCEL_RANGE ADXL345_RANGE_16G +#endif +PRINT_CONFIG_VAR(UMARIM_ACCEL_RANGE) + #ifndef UMARIM_ACCEL_RATE #define UMARIM_ACCEL_RATE ADXL345_RATE_50HZ #endif @@ -74,6 +79,7 @@ void imu_impl_init(void) adxl345_i2c_init(&imu_umarim.adxl, &(IMU_UMARIM_I2C_DEV), ADXL345_ADDR_ALT); // change the default data rate imu_umarim.adxl.config.rate = UMARIM_ACCEL_RATE; + imu_umarim.adxl.config.range = UMARIM_ACCEL_RANGE; imu_umarim.gyr_valid = FALSE; imu_umarim.acc_valid = FALSE; 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/ap_downlink.h b/sw/airborne/firmwares/fixedwing/ap_downlink.h index c22d64d773..dc7ede333a 100644 --- a/sw/airborne/firmwares/fixedwing/ap_downlink.h +++ b/sw/airborne/firmwares/fixedwing/ap_downlink.h @@ -47,6 +47,9 @@ #include "messages.h" #include "generated/periodic_telemetry.h" +// I2C Error counters +#include "mcu_periph/i2c.h" + #if defined DOWNLINK #define Downlink(x) x #else @@ -150,6 +153,11 @@ #define PERIODIC_SEND_SEGMENT(_trans, _dev) if (nav_in_segment) { DOWNLINK_SEND_SEGMENT(_trans, _dev, &nav_segment_x_1, &nav_segment_y_1, &nav_segment_x_2, &nav_segment_y_2); } +#if USE_IMU_FLOAT +# include "subsystems/imu.h" +# define PERIODIC_SEND_IMU_ACCEL(_trans, _dev) { DOWNLINK_SEND_IMU_ACCEL(_trans, _dev, &imuf.accel.x, &imuf.accel.y, &imuf.accel.z)} +# define PERIODIC_SEND_IMU_GYRO(_trans, _dev) { DOWNLINK_SEND_IMU_GYRO(_trans, _dev, &imuf.gyro.p, &imuf.gyro.q, &imuf.gyro.r)} +#else #ifdef IMU_TYPE_H # ifdef INS_MODULE_H # include "modules/ins/ins_module.h" @@ -180,6 +188,7 @@ # define PERIODIC_SEND_IMU_GYRO(_trans, _dev) {} # define PERIODIC_SEND_IMU_MAG(_trans, _dev) {} #endif +#endif #ifdef IMU_ANALOG #define PERIODIC_SEND_IMU(_trans, _dev) { int16_t dummy = 42; DOWNLINK_SEND_IMU(_trans, _dev, &(from_fbw.euler_dot[0]), &(from_fbw.euler_dot[1]), &(from_fbw.euler_dot[2]), &dummy, &dummy, &dummy); } @@ -317,5 +326,157 @@ #include "firmwares/fixedwing/stabilization/stabilization_adaptive.h" #define PERIODIC_SEND_H_CTL_A(_trans, _dev) DOWNLINK_SEND_H_CTL_A(_trans, _dev, &h_ctl_roll_sum_err, &h_ctl_ref_roll_angle, &h_ctl_pitch_sum_err, &h_ctl_ref_pitch_angle) +#ifdef USE_GX3 +#define PERIODIC_SEND_GX3_INFO(_trans, _dev) DOWNLINK_SEND_GX3_INFO(_trans, _dev,\ + &ahrs_impl.GX3_freq, \ + &ahrs_impl.GX3_packet.chksm_error, \ + &ahrs_impl.GX3_packet.hdr_error, \ + &ahrs_impl.GX3_chksm) +#else +#define PERIODIC_SEND_GX3_INFO(_trans, _dev) {} +#endif + +#ifdef USE_I2C0 +#define PERIODIC_SEND_I2C0_ERRORS(_trans, _dev) { \ + 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; \ + uint16_t i2c0_over_under_cnt = i2c0.errors->over_under_cnt; \ + uint16_t i2c0_pec_recep_cnt = i2c0.errors->pec_recep_cnt; \ + uint16_t i2c0_timeout_tlow_cnt = i2c0.errors->timeout_tlow_cnt; \ + uint16_t i2c0_smbus_alert_cnt = i2c0.errors->smbus_alert_cnt; \ + uint16_t i2c0_unexpected_event_cnt = i2c0.errors->unexpected_event_cnt; \ + uint32_t i2c0_last_unexpected_event = i2c0.errors->last_unexpected_event; \ + const uint8_t _bus0 = 0; \ + DOWNLINK_SEND_I2C_ERRORS(_trans, _dev, \ + &i2c0_queue_full_cnt, \ + &i2c0_ack_fail_cnt, \ + &i2c0_miss_start_stop_cnt, \ + &i2c0_arb_lost_cnt, \ + &i2c0_over_under_cnt, \ + &i2c0_pec_recep_cnt, \ + &i2c0_timeout_tlow_cnt, \ + &i2c0_smbus_alert_cnt, \ + &i2c0_unexpected_event_cnt, \ + &i2c0_last_unexpected_event, \ + &_bus0); \ + } +#else +#define PERIODIC_SEND_I2C0_ERRORS(_trans, _dev) {} +#endif + +#ifdef USE_I2C1 +#define PERIODIC_SEND_I2C1_ERRORS(_trans, _dev) { \ + 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; \ + uint16_t i2c1_over_under_cnt = i2c1.errors->over_under_cnt; \ + uint16_t i2c1_pec_recep_cnt = i2c1.errors->pec_recep_cnt; \ + uint16_t i2c1_timeout_tlow_cnt = i2c1.errors->timeout_tlow_cnt; \ + uint16_t i2c1_smbus_alert_cnt = i2c1.errors->smbus_alert_cnt; \ + uint16_t i2c1_unexpected_event_cnt = i2c1.errors->unexpected_event_cnt; \ + uint32_t i2c1_last_unexpected_event = i2c1.errors->last_unexpected_event; \ + const uint8_t _bus1 = 1; \ + DOWNLINK_SEND_I2C_ERRORS(_trans, _dev, \ + &i2c1_queue_full_cnt, \ + &i2c1_ack_fail_cnt, \ + &i2c1_miss_start_stop_cnt, \ + &i2c1_arb_lost_cnt, \ + &i2c1_over_under_cnt, \ + &i2c1_pec_recep_cnt, \ + &i2c1_timeout_tlow_cnt, \ + &i2c1_smbus_alert_cnt, \ + &i2c1_unexpected_event_cnt, \ + &i2c1_last_unexpected_event, \ + &_bus1); \ + } +#else +#define PERIODIC_SEND_I2C1_ERRORS(_trans, _dev) {} +#endif + +#ifdef USE_I2C2 +#define PERIODIC_SEND_I2C2_ERRORS(_trans, _dev) { \ + 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; \ + uint16_t i2c2_over_under_cnt = i2c2.errors->over_under_cnt; \ + uint16_t i2c2_pec_recep_cnt = i2c2.errors->pec_recep_cnt; \ + uint16_t i2c2_timeout_tlow_cnt = i2c2.errors->timeout_tlow_cnt; \ + uint16_t i2c2_smbus_alert_cnt = i2c2.errors->smbus_alert_cnt; \ + uint16_t i2c2_unexpected_event_cnt = i2c2.errors->unexpected_event_cnt; \ + uint32_t i2c2_last_unexpected_event = i2c2.errors->last_unexpected_event; \ + const uint8_t _bus2 = 2; \ + DOWNLINK_SEND_I2C_ERRORS(_trans, _dev, \ + &i2c2_queue_full_cnt, \ + &i2c2_ack_fail_cnt, \ + &i2c2_miss_start_stop_cnt, \ + &i2c2_arb_lost_cnt, \ + &i2c2_over_under_cnt, \ + &i2c2_pec_recep_cnt, \ + &i2c2_timeout_tlow_cnt, \ + &i2c2_smbus_alert_cnt, \ + &i2c2_unexpected_event_cnt, \ + &i2c2_last_unexpected_event, \ + &_bus2); \ + } +#else +#define PERIODIC_SEND_I2C2_ERRORS(_trans, _dev) {} +#endif + +#ifdef USE_I2C3 +#define PERIODIC_SEND_I2C3_ERRORS(_trans, _dev) { \ + 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(_trans, _dev, \ + &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); \ + } +#else +#define PERIODIC_SEND_I2C3_ERRORS(_trans, _dev) {} +#endif + +#ifndef SITL +#define PERIODIC_SEND_I2C_ERRORS(_trans, _dev) { \ + static uint8_t _i2c_nb_cnt = 0; \ + switch (_i2c_nb_cnt) { \ + case 0: \ + PERIODIC_SEND_I2C0_ERRORS(_trans, _dev); break; \ + case 1: \ + PERIODIC_SEND_I2C1_ERRORS(_trans, _dev); break; \ + case 2: \ + PERIODIC_SEND_I2C2_ERRORS(_trans, _dev); break; \ + case 3: \ + PERIODIC_SEND_I2C3_ERRORS(_trans, _dev); break; \ + default: \ + break; \ + } \ + _i2c_nb_cnt++; \ + if (_i2c_nb_cnt == 3) \ + _i2c_nb_cnt = 0; \ + } +#else +#define PERIODIC_SEND_I2C_ERRORS(_trans, _dev) {} +#endif #endif /* AP_DOWNLINK_H */ diff --git a/sw/airborne/firmwares/fixedwing/datalink.c b/sw/airborne/firmwares/fixedwing/datalink.c index cd76c2aa39..1a5d616c12 100644 --- a/sw/airborne/firmwares/fixedwing/datalink.c +++ b/sw/airborne/firmwares/fixedwing/datalink.c @@ -74,6 +74,9 @@ uint8_t joystick_block; } #endif +#if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK +#include "subsystems/radio_control/rc_datalink.h" +#endif #define MOfCm(_x) (((float)(_x))/100.) diff --git a/sw/airborne/firmwares/fixedwing/fbw_downlink.h b/sw/airborne/firmwares/fixedwing/fbw_downlink.h index 061a7334ed..7fde1f3140 100644 --- a/sw/airborne/firmwares/fixedwing/fbw_downlink.h +++ b/sw/airborne/firmwares/fixedwing/fbw_downlink.h @@ -66,6 +66,14 @@ PPM_NB_CHANNEL, \ ppm_pulses_usec); \ } +#elif defined RADIO_CONTROL_TYPE_SBUS +#include "subsystems/radio_control/sbus.h" +#define PERIODIC_SEND_PPM(_trans, _dev) { \ + DOWNLINK_SEND_PPM(_trans, _dev, \ + &radio_control.frame_rate, \ + SBUS_NB_CHANNEL, \ + sbus.pulses); \ +} #else #define PERIODIC_SEND_PPM(_trans, _dev) {} #endif 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 08a3c1342a..3ad0007e58 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) @@ -155,6 +172,10 @@ void init_ap( void ) { mcu_init(); #endif /* SINGLE_MCU */ + /****** initialize and reset state interface ********/ + + stateInit(); + /************* Sensors initialization ***************/ #if USE_GPS gps_init(); @@ -162,6 +183,9 @@ void init_ap( void ) { #if USE_IMU imu_init(); +#if USE_IMU_FLOAT + imu_float_init(); +#endif #endif #if USE_AHRS_ALIGNER @@ -178,8 +202,6 @@ void init_ap( void ) { ins_init(); - stateInit(); - /************* Links initialization ***************/ #if defined MCU_SPI_LINK || defined MCU_UART_LINK link_mcu_init(); @@ -228,6 +250,13 @@ void init_ap( void ) { #ifdef TRAFFIC_INFO 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; } @@ -550,7 +579,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 @@ -647,7 +676,6 @@ void event_task_ap( void ) { if (new_ins_attitude > 0) { attitude_loop(); - //LED_TOGGLE(3); new_ins_attitude = 0; } #endif @@ -696,10 +724,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; @@ -712,6 +736,7 @@ static inline void on_gyro_event( void ) { _reduced_propagation_rate++; if (_reduced_propagation_rate < (PERIODIC_FREQUENCY / AHRS_PROPAGATE_FREQUENCY)) { + return; } else { @@ -733,13 +758,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 234613fa8d..701a644b82 100644 --- a/sw/airborne/firmwares/fixedwing/main_fbw.c +++ b/sw/airborne/firmwares/fixedwing/main_fbw.c @@ -57,17 +57,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; @@ -106,10 +102,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 ae91f2c2d1..adf1490c9b 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot.c @@ -49,10 +49,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" @@ -60,6 +75,7 @@ static inline int ahrs_is_aligned(void) { return (ahrs.status == AHRS_RUNNING); } #else +PRINT_CONFIG_MSG("Using AUTOPILOT_DISABLE_AHRS_KILL") static inline int ahrs_is_aligned(void) { return TRUE; } @@ -67,20 +83,29 @@ 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 +#define MODE_STARTUP AP_MODE_KILL +PRINT_CONFIG_MSG("Using default AP_MODE_KILL as MODE_STARTUP") #endif void autopilot_init(void) { + /* mode is finally set at end of init if MODE_STARTUP is not KILL */ autopilot_mode = AP_MODE_KILL; autopilot_motors_on = FALSE; kill_throttle = ! autopilot_motors_on; 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; @@ -88,58 +113,50 @@ void autopilot_init(void) { #ifdef POWER_SWITCH_LED LED_ON(POWER_SWITCH_LED); // POWER OFF #endif + autopilot_arming_init(); -} + nav_init(); + guidance_h_init(); + guidance_v_init(); + stabilization_init(); -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; - } - } - } + /* set startup mode, propagates through to guidance h/v */ + autopilot_set_mode(MODE_STARTUP); } 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 { @@ -148,10 +165,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); - } } @@ -171,7 +184,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); @@ -215,6 +227,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: @@ -250,29 +264,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; } } @@ -325,8 +347,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 44d11f3b29..842f3ccf21 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -139,25 +139,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: @@ -249,12 +259,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); @@ -263,6 +273,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); } @@ -358,8 +369,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 2287d88252..23f625b442 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); void guidance_v_init(void) { @@ -104,9 +118,8 @@ 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(); } @@ -163,8 +176,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) { @@ -202,6 +221,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 @@ -216,17 +236,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 @@ -245,12 +269,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); @@ -271,23 +318,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 544073e355..cb04be854d 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -77,14 +77,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 @@ -138,12 +138,9 @@ STATIC_INLINE void main_init( void ) { baro_init(); imu_init(); - autopilot_init(); - nav_init(); - guidance_h_init(); - guidance_v_init(); - stabilization_init(); - +#if USE_IMU_FLOAT + imu_float_init(); +#endif ahrs_aligner_init(); ahrs_init(); @@ -153,6 +150,8 @@ STATIC_INLINE void main_init( void ) { gps_init(); #endif + autopilot_init(); + modules_init(); settings_init(); @@ -223,8 +222,17 @@ 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 && #if NO_GPS_LOST_WITH_RC_VALID radio_control.status != RC_OK && #endif @@ -233,6 +241,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 +294,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 +315,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 f78723abbf..366ff9d010 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/sw/airborne/firmwares/rotorcraft/navigation.c @@ -336,9 +336,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 b2f9f24178..3a5df19417 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c @@ -86,8 +86,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 f435e97a1f..a86f9c4c80 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c @@ -102,8 +102,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 773bd481e8..f709f9f5c0 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c @@ -92,11 +92,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)) @@ -152,9 +226,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/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index b35591458d..808a6371a6 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -134,6 +134,28 @@ #define PERIODIC_SEND_PPM(_trans, _dev) {} #endif +#ifdef USE_SUPERBITRF +#include "subsystems/datalink/superbitrf.h" +#define PERIODIC_SEND_SUPERBITRF(_trans, _dev) { \ + DOWNLINK_SEND_SUPERBITRF(_trans, _dev, \ + &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);} +#else +#define PERIODIC_SEND_SUPERBITRF(_trans, _dev) {} +#endif + #ifdef ACTUATORS #define PERIODIC_SEND_ACTUATORS(_trans, _dev) DOWNLINK_SEND_ACTUATORS(_trans, _dev, ACTUATORS_NB, actuators) #else @@ -274,7 +296,7 @@ #define PERIODIC_SEND_STAB_ATTITUDE(_trans, _dev) { \ struct FloatRates* body_rate = stateGetBodyRates_f(); \ struct FloatEulers* att = stateGetNedToBodyEulers_f(); \ - float foo; \ + float foo = 0.0; \ DOWNLINK_SEND_STAB_ATTITUDE_FLOAT(_trans, _dev, \ &(body_rate->p), &(body_rate->q), &(body_rate->r), \ &(att->phi), &(att->theta), &(att->psi), \ @@ -396,72 +418,6 @@ #define PERIODIC_SEND_AHRS_GYRO_BIAS_INT(_trans, _dev) {} #endif -#if USE_AHRS_LKF -#include "subsystems/ahrs.h" -#include "ahrs/ahrs_float_lkf.h" -#define PERIODIC_SEND_AHRS_LKF(_trans, _dev) { \ - DOWNLINK_SEND_AHRS_LKF(&bafl_eulers.phi, \ - _trans, _dev, \ - &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); \ - } -#define PERIODIC_SEND_AHRS_LKF_DEBUG(_trans, _dev) { \ - DOWNLINK_SEND_AHRS_LKF_DEBUG(_trans, _dev, \ - &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]); \ - } -#define PERIODIC_SEND_AHRS_LKF_ACC_DBG(_trans, _dev) { \ - DOWNLINK_SEND_AHRS_LKF_ACC_DBG(_trans, _dev, \ - &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); \ - } -#define PERIODIC_SEND_AHRS_LKF_MAG_DBG(_trans, _dev) { \ - DOWNLINK_SEND_AHRS_LKF_MAG_DBG(_trans, _dev, \ - &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); \ - } -#else -#define PERIODIC_SEND_AHRS_LKF(_trans, _dev) {} -#define PERIODIC_SEND_AHRS_LKF_DEBUG(_trans, _dev) {} -#define PERIODIC_SEND_AHRS_LKF_MAG_DBG(_trans, _dev) {} -#define PERIODIC_SEND_AHRS_LKF_ACC_DBG(_trans, _dev) {} -#endif #if defined STABILIZATION_ATTITUDE_TYPE_QUAT && defined STABILIZATION_ATTITUDE_TYPE_INT #define PERIODIC_SEND_AHRS_REF_QUAT(_trans, _dev) { \ @@ -690,6 +646,14 @@ &guidance_v_delta_t); \ } +#define PERIODIC_SEND_TUNE_VERT(_trans, _dev) { \ + DOWNLINK_SEND_TUNE_VERT(_trans, _dev, \ + &guidance_v_z_sp, \ + &ins_ltp_pos.z, \ + &guidance_v_z_ref, \ + &guidance_v_zd_ref); \ + } + #define PERIODIC_SEND_HOVER_LOOP(_trans, _dev) { \ DOWNLINK_SEND_HOVER_LOOP(_trans, _dev, \ &guidance_h_pos_sp.x, \ @@ -840,6 +804,7 @@ #ifdef USE_I2C0 #define PERIODIC_SEND_I2C0_ERRORS(_trans, _dev) { \ + 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; \ @@ -851,6 +816,7 @@ uint32_t i2c0_last_unexpected_event = i2c0.errors->last_unexpected_event; \ const uint8_t _bus0 = 0; \ DOWNLINK_SEND_I2C_ERRORS(_trans, _dev, \ + &i2c0_queue_full_cnt, \ &i2c0_ack_fail_cnt, \ &i2c0_miss_start_stop_cnt, \ &i2c0_arb_lost_cnt, \ @@ -868,6 +834,7 @@ #ifdef USE_I2C1 #define PERIODIC_SEND_I2C1_ERRORS(_trans, _dev) { \ + 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; \ @@ -879,6 +846,7 @@ uint32_t i2c1_last_unexpected_event = i2c1.errors->last_unexpected_event; \ const uint8_t _bus1 = 1; \ DOWNLINK_SEND_I2C_ERRORS(_trans, _dev, \ + &i2c1_queue_full_cnt, \ &i2c1_ack_fail_cnt, \ &i2c1_miss_start_stop_cnt, \ &i2c1_arb_lost_cnt, \ @@ -896,6 +864,7 @@ #ifdef USE_I2C2 #define PERIODIC_SEND_I2C2_ERRORS(_trans, _dev) { \ + 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; \ @@ -907,6 +876,7 @@ uint32_t i2c2_last_unexpected_event = i2c2.errors->last_unexpected_event; \ const uint8_t _bus2 = 2; \ DOWNLINK_SEND_I2C_ERRORS(_trans, _dev, \ + &i2c2_queue_full_cnt, \ &i2c2_ack_fail_cnt, \ &i2c2_miss_start_stop_cnt, \ &i2c2_arb_lost_cnt, \ @@ -922,6 +892,36 @@ #define PERIODIC_SEND_I2C2_ERRORS(_trans, _dev) {} #endif +#ifdef USE_I2C3 +#define PERIODIC_SEND_I2C3_ERRORS(_trans, _dev) { \ + 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(_trans, _dev, \ + &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); \ + } +#else +#define PERIODIC_SEND_I2C3_ERRORS(_trans, _dev) {} +#endif + #ifndef SITL #define PERIODIC_SEND_I2C_ERRORS(_trans, _dev) { \ static uint8_t _i2c_nb_cnt = 0; \ @@ -932,6 +932,8 @@ PERIODIC_SEND_I2C1_ERRORS(_trans, _dev); break; \ case 2: \ PERIODIC_SEND_I2C2_ERRORS(_trans, _dev); break; \ + case 3: \ + PERIODIC_SEND_I2C3_ERRORS(_trans, _dev); break; \ default: \ break; \ } \ @@ -962,34 +964,35 @@ #ifdef ARDRONE2_RAW #include "navdata.h" #define PERIODIC_SEND_ARDRONE_NAVDATA(_trans, _dev) DOWNLINK_SEND_ARDRONE_NAVDATA(_trans, _dev, \ - &navdata->taille, \ - &navdata->nu_trame, \ - &navdata->ax, \ - &navdata->ay, \ - &navdata->az, \ - &navdata->vx, \ - &navdata->vy, \ - &navdata->vz, \ - &navdata->temperature_acc, \ - &navdata->temperature_gyro, \ - &navdata->ultrasound, \ - &navdata->us_debut_echo, \ - &navdata->us_fin_echo, \ - &navdata->us_association_echo, \ - &navdata->us_distance_echo, \ - &navdata->us_curve_time, \ - &navdata->us_curve_value, \ - &navdata->us_curve_ref, \ - &navdata->nb_echo, \ - &navdata->sum_echo, \ - &navdata->gradient, \ - &navdata->flag_echo_ini, \ - &navdata->pressure, \ - &navdata->temperature_pressure, \ - &navdata->mx, \ - &navdata->my, \ - &navdata->mz, \ - &navdata->chksum \ + &navdata.taille, \ + &navdata.nu_trame, \ + &navdata.ax, \ + &navdata.ay, \ + &navdata.az, \ + &navdata.vx, \ + &navdata.vy, \ + &navdata.vz, \ + &navdata.temperature_acc, \ + &navdata.temperature_gyro, \ + &navdata.ultrasound, \ + &navdata.us_debut_echo, \ + &navdata.us_fin_echo, \ + &navdata.us_association_echo, \ + &navdata.us_distance_echo, \ + &navdata.us_curve_time, \ + &navdata.us_curve_value, \ + &navdata.us_curve_ref, \ + &navdata.nb_echo, \ + &navdata.sum_echo, \ + &navdata.gradient, \ + &navdata.flag_echo_ini, \ + &navdata.pressure, \ + &navdata.temperature_pressure, \ + &navdata.mx, \ + &navdata.my, \ + &navdata.mz, \ + &navdata.chksum, \ + &nav_port.checksum_errors \ ) #else #define PERIODIC_SEND_ARDRONE_NAVDATA(_trans, _dev) {} @@ -1004,10 +1007,13 @@ #ifdef USE_UART1 #define PERIODIC_SEND_UART1_ERRORS(_trans, _dev) { \ const uint8_t _bus1 = 1; \ + uint16_t ore = uart1.ore; \ + uint16_t ne_err = uart1.ne_err; \ + uint16_t fe_err = uart1.fe_err; \ DOWNLINK_SEND_UART_ERRORS(_trans, _dev, \ - &uart1.ore, \ - &uart1.ne_err, \ - &uart1.fe_err, \ + &ore, \ + &ne_err, \ + &fe_err, \ &_bus1); \ } #else @@ -1017,10 +1023,13 @@ #ifdef USE_UART2 #define PERIODIC_SEND_UART2_ERRORS(_trans, _dev) { \ const uint8_t _bus2 = 2; \ + uint16_t ore = uart2.ore; \ + uint16_t ne_err = uart2.ne_err; \ + uint16_t fe_err = uart2.fe_err; \ DOWNLINK_SEND_UART_ERRORS(_trans, _dev, \ - &uart2.ore, \ - &uart2.ne_err, \ - &uart2.fe_err, \ + &ore, \ + &ne_err, \ + &fe_err, \ &_bus2); \ } #else @@ -1030,10 +1039,13 @@ #ifdef USE_UART3 #define PERIODIC_SEND_UART3_ERRORS(_trans, _dev) { \ const uint8_t _bus3 = 3; \ + uint16_t ore = uart3.ore; \ + uint16_t ne_err = uart3.ne_err; \ + uint16_t fe_err = uart3.fe_err; \ DOWNLINK_SEND_UART_ERRORS(_trans, _dev, \ - &uart3.ore, \ - &uart3.ne_err, \ - &uart3.fe_err, \ + &ore, \ + &ne_err, \ + &fe_err, \ &_bus3); \ } #else @@ -1043,10 +1055,13 @@ #ifdef USE_UART5 #define PERIODIC_SEND_UART5_ERRORS(_trans, _dev) { \ const uint8_t _bus5 = 5; \ + uint16_t ore = uart5.ore; \ + uint16_t ne_err = uart5.ne_err; \ + uint16_t fe_err = uart5.fe_err; \ DOWNLINK_SEND_UART_ERRORS(_trans, _dev, \ - &uart5.ore, \ - &uart5.ne_err, \ - &uart5.fe_err, \ + &ore, \ + &ne_err, \ + &fe_err, \ &_bus5); \ } #else @@ -1076,4 +1091,14 @@ #define PERIODIC_SEND_UART_ERRORS(_trans, _dev) {} #endif +#ifdef USE_GX3 +#define PERIODIC_SEND_GX3_INFO(_trans, _dev) DOWNLINK_SEND_GX3_INFO(_trans, _dev,\ + &ahrs_impl.GX3_freq, \ + &ahrs_impl.GX3_packet.chksm_error, \ + &ahrs_impl.GX3_packet.hdr_error, \ + &ahrs_impl.GX3_chksm) +#else +#define PERIODIC_SEND_GX3_INFO(_trans, _dev) {} +#endif + #endif /* TELEMETRY_H */ diff --git a/sw/airborne/led.h b/sw/airborne/led.h index 46e8dc662f..f9aa9e2c55 100644 --- a/sw/airborne/led.h +++ b/sw/airborne/led.h @@ -74,6 +74,16 @@ static inline void led_init ( void ) { LED_OFF(8); #endif /* LED_8 */ +#if USE_LED_9 + LED_INIT(9); + LED_OFF(9); +#endif /* LED_9 */ + +#if USE_LED_10 + LED_INIT(10); + LED_OFF(10); +#endif /* LED_10 */ + #ifdef USE_LED_BODY LED_INIT(BODY); LED_OFF(BODY); 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 57991a715b..0562689d24 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); } @@ -111,11 +92,40 @@ void enu_of_ecef_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct Ece void ned_of_ecef_point_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef) { - struct EnuCoor_i enu; enu_of_ecef_point_i(&enu, def, ecef); ENU_OF_TO_NED(*ned, enu); +} + +/** Convert a ECEF position to local ENU. + * @param[out] enu ENU position in meter << #INT32_POS_FRAC + * @param[in] def local coordinate system definition + * @param[in] ecef ECEF position in cm + */ +void enu_of_ecef_pos_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef) { + struct EnuCoor_i enu_cm; + enu_of_ecef_point_i(&enu_cm, def, ecef); + + /* enu = (enu_cm / 100) << INT32_POS_FRAC + * to loose less range: + * enu_cm = enu << (INT32_POS_FRAC-2) / 25 + * which puts max enu output Q23.8 range to 8388km / 25 = 335km + */ + INT32_VECT3_LSHIFT(*enu, enu_cm, INT32_POS_FRAC-2); + VECT3_SDIV(*enu, *enu, 25); +} + + +/** Convert a ECEF position to local NED. + * @param[out] ned NED position in meter << #INT32_POS_FRAC + * @param[in] def local coordinate system definition + * @param[in] ecef ECEF position in cm + */ +void ned_of_ecef_pos_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef) { + struct EnuCoor_i enu; + enu_of_ecef_pos_i(&enu, def, ecef); + ENU_OF_TO_NED(*ned, enu); } void enu_of_ecef_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef) { @@ -142,20 +152,24 @@ void ned_of_ecef_vect_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct Ecef ENU_OF_TO_NED(*ned, enu); } -/* check if resolution of INT32_TRIG_FRAC (14) is enough here */ -void ecef_of_enu_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu) { - INT32_RMAT_TRANSP_VMULT(*ecef, def->ltp_of_ecef, *enu); - INT32_VECT3_ADD(*ecef, def->ecef); -} - -void ecef_of_ned_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned) { - struct EnuCoor_i enu; - ENU_OF_TO_NED(enu, *ned); - ecef_of_enu_point_i(ecef, def, &enu); -} void ecef_of_enu_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu) { - INT32_RMAT_TRANSP_VMULT(*ecef, def->ltp_of_ecef, *enu); + + const int64_t tmpx = (int64_t)def->ltp_of_ecef.m[0] * enu->x + + (int64_t)def->ltp_of_ecef.m[3] * enu->y + + (int64_t)def->ltp_of_ecef.m[6] * enu->z; + ecef->x = (int32_t)(tmpx>>HIGH_RES_TRIG_FRAC); + + const int64_t tmpy = (int64_t)def->ltp_of_ecef.m[1] * enu->x + + (int64_t)def->ltp_of_ecef.m[4] * enu->y + + (int64_t)def->ltp_of_ecef.m[7] * enu->z; + ecef->y = (int32_t)(tmpy>>HIGH_RES_TRIG_FRAC); + + /* first element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ENU_to_ECEF */ + const int64_t tmpz = (int64_t)def->ltp_of_ecef.m[5] * enu->y + + (int64_t)def->ltp_of_ecef.m[8] * enu->z; + ecef->z = (int32_t)(tmpz>>HIGH_RES_TRIG_FRAC); + } void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned) { @@ -165,6 +179,60 @@ void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct Ne } +/** Convert a point in local ENU to ECEF. + * @param[out] ecef ECEF point in cm + * @param[in] def local coordinate system definition + * @param[in] enu ENU point in cm + */ +void ecef_of_enu_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu) { + ecef_of_enu_vect_i(ecef, def, enu); + INT32_VECT3_ADD(*ecef, def->ecef); +} + + +/** Convert a point in local NED to ECEF. + * @param[out] ecef ECEF point in cm + * @param[in] def local coordinate system definition + * @param[in] ned NED point in cm + */ +void ecef_of_ned_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned) { + struct EnuCoor_i enu; + ENU_OF_TO_NED(enu, *ned); + ecef_of_enu_point_i(ecef, def, &enu); +} + + +/** Convert a local ENU position to ECEF. + * @param[out] ecef ECEF position in cm + * @param[in] def local coordinate system definition + * @param[in] enu ENU position in meter << #INT32_POS_FRAC + */ +void ecef_of_enu_pos_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu) { + /* enu_cm = (enu * 100) >> INT32_POS_FRAC + * to loose less range: + * enu_cm = (enu * 25) >> (INT32_POS_FRAC-2) + * which puts max enu input Q23.8 range to 8388km / 25 = 335km + */ + struct EnuCoor_i enu_cm; + VECT3_SMUL(enu_cm, *enu, 25); + INT32_VECT3_RSHIFT(enu_cm, enu_cm, INT32_POS_FRAC-2); + ecef_of_enu_vect_i(ecef, def, &enu_cm); + INT32_VECT3_ADD(*ecef, def->ecef); +} + + +/** Convert a local NED position to ECEF. + * @param[out] ecef ECEF position in cm + * @param[in] def local coordinate system definition + * @param[in] ned NED position in meter << #INT32_POS_FRAC + */ +void ecef_of_ned_pos_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned) { + struct EnuCoor_i enu; + ENU_OF_TO_NED(enu, *ned); + ecef_of_enu_pos_i(ecef, def, &enu); +} + + void enu_of_lla_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct LlaCoor_i* lla) { struct EcefCoor_i ecef; ecef_of_lla_i(&ecef,lla); diff --git a/sw/airborne/math/pprz_geodetic_int.h b/sw/airborne/math/pprz_geodetic_int.h index 9d95d52319..cbeac4ac5f 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,12 +97,15 @@ 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); extern void ecef_of_lla_i(struct EcefCoor_i* out, struct LlaCoor_i* in); extern void enu_of_ecef_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef); extern void ned_of_ecef_point_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef); +extern void enu_of_ecef_pos_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef); +extern void ned_of_ecef_pos_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef); extern void enu_of_ecef_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef); extern void ned_of_ecef_vect_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef); extern void enu_of_lla_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct LlaCoor_i* lla); @@ -113,6 +114,8 @@ extern void enu_of_lla_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struc extern void ned_of_lla_vect_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct LlaCoor_i* lla); extern void ecef_of_enu_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu); extern void ecef_of_ned_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned); +extern void ecef_of_enu_pos_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu); +extern void ecef_of_ned_pos_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned); extern void ecef_of_enu_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu); extern void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned); 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 897278d41c..15dca0d1e5 100644 --- a/sw/airborne/mcu_periph/i2c.c +++ b/sw/airborne/mcu_periph/i2c.c @@ -62,6 +62,19 @@ void i2c2_init(void) { #endif /* USE_I2C2 */ + +#ifdef USE_I2C3 + +struct i2c_periph i2c3; + +void i2c3_init(void) { + i2c_init(&i2c3); + i2c3_hw_init(); +} + +#endif /* USE_I2C2 */ + + 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 1776ae2ca5..13c1af34a6 100644 --- a/sw/airborne/mcu_periph/uart.h +++ b/sw/airborne/mcu_periph/uart.h @@ -36,19 +36,18 @@ #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 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 + +#define USTOP_1 1 +#define USTOP_2 2 + +#define UPARITY_NO 0 +#define UPARITY_ODD 1 +#define UPARITY_EVEN 2 /** * UART peripheral @@ -75,6 +74,7 @@ struct uart_periph { extern void uart_periph_init(struct uart_periph* p); extern void uart_periph_set_baudrate(struct uart_periph* p, uint32_t baud); +extern void uart_periph_set_bits_stop_parity(struct uart_periph* p, uint8_t bits, uint8_t stop, uint8_t parity); extern void uart_periph_set_mode(struct uart_periph* p, bool_t tx_enabled, bool_t rx_enabled, bool_t hw_flow_control); extern void uart_transmit(struct uart_periph* p, uint8_t data); extern bool_t uart_check_free_space(struct uart_periph* p, uint8_t len); @@ -97,6 +97,7 @@ extern void uart0_init(void); #define UART0Getch() uart_getch(&uart0) #define UART0TxRunning uart0.tx_running #define UART0SetBaudrate(_b) uart_periph_set_baudrate(&uart0, _b) +#define UART0SetBitsStopParity(_b, _s, _p) uart_periph_set_bits_stop_parity(&uart0, _b, _s, _p) #endif // USE_UART0 @@ -112,6 +113,7 @@ extern void uart1_init(void); #define UART1Getch() uart_getch(&uart1) #define UART1TxRunning uart1.tx_running #define UART1SetBaudrate(_b) uart_periph_set_baudrate(&uart1, _b) +#define UART1SetBitsStopParity(_b, _s, _p) uart_periph_set_bits_stop_parity(&uart1, _b, _s, _p) #endif // USE_UART1 @@ -127,6 +129,7 @@ extern void uart2_init(void); #define UART2Getch() uart_getch(&uart2) #define UART2TxRunning uart2.tx_running #define UART2SetBaudrate(_b) uart_periph_set_baudrate(&uart2, _b) +#define UART2SetBitsStopParity(_b, _s, _p) uart_periph_set_bits_stop_parity(&uart2, _b, _s, _p) #endif // USE_UART2 @@ -142,6 +145,7 @@ extern void uart3_init(void); #define UART3Getch() uart_getch(&uart3) #define UART3TxRunning uart3.tx_running #define UART3SetBaudrate(_b) uart_periph_set_baudrate(&uart3, _b) +#define UART3SetBitsStopParity(_b, _s, _p) uart_periph_set_bits_stop_parity(&uart3, _b, _s, _p) #endif // USE_UART3 @@ -157,6 +161,7 @@ extern void uart4_init(void); #define UART4Getch() uart_getch(&uart4) #define UART4TxRunning uart4.tx_running #define UART4SetBaudrate(_b) uart_periph_set_baudrate(&uart4, _b) +#define UART4SetBitsStopParity(_b, _s, _p) uart_periph_set_bits_stop_parity(&uart4, _b, _s, _p) #endif // USE_UART4 @@ -172,6 +177,7 @@ extern void uart5_init(void); #define UART5Getch() uart_getch(&uart5) #define UART5TxRunning uart5.tx_running #define UART5SetBaudrate(_b) uart_periph_set_baudrate(&uart5, _b) +#define UART5SetBitsStopParity(_b, _s, _p) uart_periph_set_bits_stop_parity(&uart5, _b, _s, _p) #endif // USE_UART5 @@ -187,6 +193,7 @@ extern void uart6_init(void); #define UART6Getch() uart_getch(&uart6) #define UART6TxRunning uart6.tx_running #define UART6SetBaudrate(_b) uart_periph_set_baudrate(&uart6, _b) +#define UART6SetBitsStopParity(_b, _s, _p) uart_periph_set_bits_stop_parity(&uart6, _b, _s, _p) #endif // USE_UART6 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 c4d3239ccd..5f76e17395 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; @@ -75,13 +77,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; } @@ -96,6 +104,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/digital_cam/led_cam_ctrl.c b/sw/airborne/modules/digital_cam/led_cam_ctrl.c index 8928381f48..81b701f6af 100644 --- a/sw/airborne/modules/digital_cam/led_cam_ctrl.c +++ b/sw/airborne/modules/digital_cam/led_cam_ctrl.c @@ -52,6 +52,12 @@ void dc_send_command(uint8_t cmd) case DC_ON: DC_PUSH(DC_POWER_LED); break; +#endif +#ifdef DC_POWER_OFF_LED + case DC_OFF: + DC_PUSH(DC_POWER_OFF_LED); + dc_timer = DC_POWER_OFF_DELAY; + break; #endif default: break; diff --git a/sw/airborne/modules/digital_cam/led_cam_ctrl.h b/sw/airborne/modules/digital_cam/led_cam_ctrl.h index 37eb0a7f17..fde5cb4aa7 100644 --- a/sw/airborne/modules/digital_cam/led_cam_ctrl.h +++ b/sw/airborne/modules/digital_cam/led_cam_ctrl.h @@ -35,6 +35,7 @@ * * * + * * @endverbatim * Related bank and pin must also be defined: * @verbatim @@ -68,6 +69,10 @@ extern uint8_t dc_timer; #define DC_SHUTTER_DELAY 2 /* 4Hz -> 0.5s */ #endif +#ifndef DC_POWER_OFF_DELAY +#define DC_POWER_OFF_DELAY 3 +#endif + #ifndef DC_SHUTTER_LED #error DC: Please specify at least a SHUTTER LED #endif @@ -90,6 +95,9 @@ static inline void led_cam_ctrl_init(void) #ifdef DC_POWER_LED DC_RELEASE(DC_POWER_LED); #endif +#ifdef DC_POWER_OFF_LED + DC_RELEASE(DC_POWER_OFF_LED); +#endif } @@ -114,6 +122,9 @@ static inline void led_cam_ctrl_periodic( void ) #endif #ifdef DC_POWER_LED DC_RELEASE(DC_POWER_LED); +#endif +#ifdef DC_POWER_OFF_LED + DC_RELEASE(DC_POWER_OFF_LED); #endif } 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/subsystems/ahrs/ahrs_float_ekf.h b/sw/airborne/modules/gumstix_interface/qr_code_spi_link.h similarity index 73% rename from sw/airborne/subsystems/ahrs/ahrs_float_ekf.h rename to sw/airborne/modules/gumstix_interface/qr_code_spi_link.h index 6d173227ba..06aa237943 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_ekf.h +++ b/sw/airborne/modules/gumstix_interface/qr_code_spi_link.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2008-2009 Antoine Drouin + * Copyright (C) 2005-2013 The Paparazzi Team * * This file is part of paparazzi. * @@ -17,19 +17,15 @@ * 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 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); -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 /* 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 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 460e7e9c46..abf946bd96 100644 --- a/sw/airborne/modules/sensors/airspeed_amsys.c +++ b/sw/airborne/modules/sensors/airspeed_amsys.c @@ -31,27 +31,21 @@ #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 #define AIRSPEED_AMSYS_SCALE 1 #endif -#ifndef AIRSPEED_AMSYS_OFFSET -#define AIRSPEED_AMSYS_OFFSET 0 -#endif #define AIRSPEED_AMSYS_OFFSET_MAX 29491 #define AIRSPEED_AMSYS_OFFSET_MIN 3277 #define AIRSPEED_AMSYS_OFFSET_NBSAMPLES_INIT 40 #define AIRSPEED_AMSYS_OFFSET_NBSAMPLES_AVRG 60 #define AIRSPEED_AMSYS_NBSAMPLES_AVRG 10 #ifndef AIRSPEED_AMSYS_MAXPRESURE -#define AIRSPEED_AMSYS_MAXPRESURE 2068//2073 //Pascal +#define AIRSPEED_AMSYS_MAXPRESURE 2068 //003-2068, 001-689 //Pascal +#endif +#ifndef AIRSPEED_AMSYS_FILTER +#define AIRSPEED_AMSYS_FILTER 0 #endif #ifndef AIRSPEED_AMSYS_I2C_DEV #define AIRSPEED_AMSYS_I2C_DEV i2c0 @@ -71,8 +65,9 @@ uint16_t airspeed_amsys_raw; uint16_t tempAS_amsys_raw; bool_t airspeed_amsys_valid; -float airspeed_tmp; -float pressure_amsys; //Pascal +float airspeed_amsys_offset; +float airspeed_amsys_tmp; +float airspeed_amsys_p; //Pascal float airspeed_amsys; //mps float airspeed_scale; float airspeed_filter; @@ -82,80 +77,138 @@ struct i2c_transaction airspeed_amsys_i2c_trans; volatile bool_t airspeed_amsys_i2c_done; float airspeed_temperature = 0.0; float airspeed_old = 0.0; +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; - pressure_amsys = 0.0; - airspeed_amsys_i2c_done = TRUE; - airspeed_amsys_valid = TRUE; - airspeed_scale = AIRSPEED_SCALE; - airspeed_filter = AIRSPEED_FILTER; - airspeed_amsys_i2c_trans.status = I2CTransDone; + 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 - -#else // SITL - extern float sim_air_speed; - stateSetAirspeed_f(&sim_air_speed); -#endif //SITL -} - -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]; -#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 -#endif - - // 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) { - - // raw not under offest min - if (airspeed_amsys_rawAIRSPEED_AMSYS_OFFSET_MAX) - airspeed_amsys_raw = AIRSPEED_AMSYS_OFFSET_MAX; - - // calculate raw to pressure - pressure_amsys = (float)(airspeed_amsys_raw-AIRSPEED_AMSYS_OFFSET_MIN)*AIRSPEED_AMSYS_MAXPRESURE/(float)(AIRSPEED_AMSYS_OFFSET_MAX-AIRSPEED_AMSYS_OFFSET_MIN); - - airspeed_tmp = sqrtf(2*(pressure_amsys)*airspeed_scale/1.2041); //without offset - - // Lowpass filter - airspeed_amsys = airspeed_filter * airspeed_old + (1 - airspeed_filter) * airspeed_tmp; - airspeed_old = airspeed_amsys; + } #if USE_AIRSPEED stateSetAirspeed_f(&airspeed_amsys); #endif -#ifdef SENSOR_SYNC_SEND - DOWNLINK_SEND_AMSYS_AIRSPEED(DefaultChannel, DefaultDevice, &airspeed_amsys_raw, &pressure_amsys, &airspeed_tmp, &airspeed_amsys, &airspeed_temperature); -#else - RunOnceEvery(10, DOWNLINK_SEND_AMSYS_AIRSPEED(DefaultChannel, DefaultDevice, &airspeed_amsys_raw, &pressure_amsys, &airspeed_tmp, &airspeed_amsys, &airspeed_temperature)); -#endif - } - // Transaction has been read - airspeed_amsys_i2c_trans.status = I2CTransDone; +#elif !defined USE_NPS + extern float sim_air_speed; + stateSetAirspeed_f(&sim_air_speed); +#endif //SITL + + +#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]; +#ifdef MEASURE_AMSYS_TEMPERATURE + 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; + + // Continue only if a new airspeed value was received + if (airspeed_amsys_valid) { + + // 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 + 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); + + 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; + } + + airspeed_amsys = 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; } diff --git a/sw/airborne/modules/sensors/airspeed_ets.c b/sw/airborne/modules/sensors/airspeed_ets.c index 92930f2495..6f803c30b3 100644 --- a/sw/airborne/modules/sensors/airspeed_ets.c +++ b/sw/airborne/modules/sensors/airspeed_ets.c @@ -42,13 +42,14 @@ #include "state.h" #include "mcu_periph/i2c.h" #include "mcu_periph/uart.h" +#include "mcu_periph/sys_time.h" #include "messages.h" #include "subsystems/datalink/downlink.h" #include #if !USE_AIRSPEED -#ifndef SENSOR_SYNC_SEND -#warning either set USE_AIRSPEED or SENSOR_SYNC_SEND to use ets_airspeed +#ifndef AIRSPEED_ETS_SYNC_SEND +#warning either set USE_AIRSPEED or AIRSPEED_ETS_SYNC_SEND to use ets_airspeed #endif #endif @@ -68,6 +69,13 @@ #ifndef AIRSPEED_ETS_I2C_DEV #define AIRSPEED_ETS_I2C_DEV i2c0 #endif +PRINT_CONFIG_VAR(AIRSPEED_ETS_I2C_DEV) + +/** delay in seconds until sensor is read after startup */ +#ifndef AIRSPEED_ETS_START_DELAY +#define AIRSPEED_ETS_START_DELAY 0.2 +#endif +PRINT_CONFIG_VAR(AIRSPEED_ETS_START_DELAY) #ifndef DOWNLINK_DEVICE #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE @@ -88,6 +96,8 @@ volatile bool_t airspeed_ets_i2c_done; bool_t airspeed_ets_offset_init; uint32_t airspeed_ets_offset_tmp; uint16_t airspeed_ets_cnt; +uint32_t airspeed_ets_delay_time; +bool_t airspeed_ets_delay_done; void airspeed_ets_init( void ) { int n; @@ -96,7 +106,7 @@ void airspeed_ets_init( void ) { airspeed_ets_offset = 0; airspeed_ets_offset_tmp = 0; airspeed_ets_i2c_done = TRUE; - airspeed_ets_valid = TRUE; + airspeed_ets_valid = FALSE; airspeed_ets_offset_init = FALSE; airspeed_ets_cnt = AIRSPEED_ETS_OFFSET_NBSAMPLES_INIT + AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG; @@ -105,13 +115,20 @@ void airspeed_ets_init( void ) { airspeed_ets_buffer[n] = 0.0; airspeed_ets_i2c_trans.status = I2CTransDone; + + airspeed_ets_delay_done = FALSE; + SysTimeTimerStart(airspeed_ets_delay_time); } void airspeed_ets_read_periodic( void ) { #ifndef SITL + if (!airspeed_ets_delay_done) { + if (SysTimeTimer(airspeed_ets_delay_time) < USEC_OF_SEC(AIRSPEED_ETS_START_DELAY)) return; + else airspeed_ets_delay_done = TRUE; + } 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 @@ -173,7 +190,7 @@ void airspeed_ets_read_event( void ) { #if USE_AIRSPEED stateSetAirspeed_f(&airspeed_ets); #endif -#ifdef SENSOR_SYNC_SEND +#ifdef AIRSPEED_ETS_SYNC_SEND DOWNLINK_SEND_AIRSPEED_ETS(DefaultChannel, DefaultDevice, &airspeed_ets_raw, &airspeed_ets_offset, &airspeed_ets); #endif } else { diff --git a/sw/airborne/modules/sensors/AOA_adc.c b/sw/airborne/modules/sensors/aoa_adc.c similarity index 62% rename from sw/airborne/modules/sensors/AOA_adc.c rename to sw/airborne/modules/sensors/aoa_adc.c index bebcbe02fb..2cb7aeb284 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,34 +37,25 @@ #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 DOWNLINK_DEVICE #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE #endif -#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 @@ -73,32 +64,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 209140ca4f..faa014ff94 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 @@ -48,12 +49,21 @@ #define BARO_AMSYS_ADDR 0xE4 #define BARO_AMSYS_REG 0x07 -#define BARO_AMSYS_SCALE 0.32 +#ifndef BARO_AMSYS_SCALE +#define BARO_AMSYS_SCALE 1 +#endif +#ifndef BARO_AMSYS_MAX_PRESSURE #define BARO_AMSYS_MAX_PRESSURE 103400 // Pascal +#endif #define BARO_AMSYS_OFFSET_MAX 29491 #define BARO_AMSYS_OFFSET_MIN 3277 #define BARO_AMSYS_OFFSET_NBSAMPLES_INIT 40 #define BARO_AMSYS_OFFSET_NBSAMPLES_AVRG 60 +#ifndef BARO_AMSYS_FILTER +#define BARO_AMSYS_FILTER 0 +#endif +#define BARO_AMSYS_R 0.5 +#define BARO_AMSYS_SIGMA2 0.1 #ifdef MEASURE_AMSYS_TEMPERATURE #define TEMPERATURE_AMSYS_OFFSET_MAX 29491 @@ -62,8 +72,6 @@ #define TEMPERATURE_AMSYS_MIN -25 #endif -//#define BARO_AMSYS_R 0.5 -//#define BARO_AMSYS_SIGMA2 0.1 //#define BARO_ALT_CORRECTION 0.0 #ifndef BARO_AMSYS_I2C_DEV #define BARO_AMSYS_I2C_DEV i2c0 @@ -76,17 +84,18 @@ uint16_t baro_amsys_adc; float baro_amsys_offset; bool_t baro_amsys_valid; float baro_amsys_altitude; +bool_t baro_amsys_enabled; +float baro_amsys_r; +float baro_amsys_sigma2; float baro_amsys_temp; float baro_amsys_p; float baro_amsys_offset_altitude; float baro_amsys_abs_altitude; float ref_alt_init; //Altitude by initialising +float baro_scale; float baro_filter; float baro_old; -//float baro_amsys_r; -//float baro_amsys_sigma2; - struct i2c_transaction baro_amsys_i2c_trans; @@ -96,106 +105,112 @@ double baro_amsys_offset_tmp; uint16_t baro_amsys_cnt; void baro_amsys_init( void ) { - baro_filter=BARO_FILTER; - pBaroRaw = 0; - tBaroRaw = 0; - baro_amsys_altitude = 0.0; - baro_amsys_p=0.0; - baro_amsys_offset = 0; - baro_amsys_offset_tmp = 0; - baro_amsys_valid = TRUE; - baro_amsys_offset_init = FALSE; -// baro_amsys_enabled = TRUE; - baro_amsys_cnt = BARO_AMSYS_OFFSET_NBSAMPLES_INIT + BARO_AMSYS_OFFSET_NBSAMPLES_AVRG; -// baro_amsys_r = BARO_AMSYS_R; -// baro_amsys_sigma2 = BARO_AMSYS_SIGMA2; -// baro_head=0; - ref_alt_init = 0; - baro_amsys_i2c_trans.status = I2CTransDone; + baro_filter=BARO_AMSYS_FILTER; + pBaroRaw = 0; + tBaroRaw = 0; + baro_amsys_altitude = 0.0; + baro_amsys_p=0.0; + baro_amsys_offset = 0; + baro_amsys_offset_tmp = 0; + baro_amsys_valid = TRUE; + baro_amsys_offset_init = FALSE; + baro_amsys_enabled = TRUE; + baro_scale = BARO_AMSYS_SCALE; + baro_amsys_cnt = BARO_AMSYS_OFFSET_NBSAMPLES_INIT + BARO_AMSYS_OFFSET_NBSAMPLES_AVRG; + baro_amsys_r = BARO_AMSYS_R; + baro_amsys_sigma2 = BARO_AMSYS_SIGMA2; + // baro_head=0; + ref_alt_init = 0; + baro_amsys_i2c_trans.status = I2CTransDone; } void baro_amsys_read_periodic( void ) { // Initiate next read #ifndef SITL - if (baro_amsys_i2c_trans.status == I2CTransDone){ + if (baro_amsys_i2c_trans.status == I2CTransDone){ #ifndef MEASURE_AMSYS_TEMPERATURE - i2c_receive(&BARO_AMSYS_I2C_DEV, &baro_amsys_i2c_trans, BARO_AMSYS_ADDR, 2); + i2c_receive(&BARO_AMSYS_I2C_DEV, &baro_amsys_i2c_trans, BARO_AMSYS_ADDR, 2); #else - i2c_receive(&BARO_AMSYS_I2C_DEV, &baro_amsys_i2c_trans, BARO_AMSYS_ADDR, 4); + i2c_receive(&BARO_AMSYS_I2C_DEV, &baro_amsys_i2c_trans, BARO_AMSYS_ADDR, 4); #endif - } + } #else // SITL - pBaroRaw = 0; - baro_amsys_altitude = gps.hmsl / 1000.0; - baro_amsys_adc = baro_amsys_altitude / BARO_AMSYS_SCALE; - baro_amsys_valid = TRUE; + /* fake an offset so sim works for under hmsl as well */ + if (!baro_amsys_offset_init) { + baro_amsys_offset = BARO_AMSYS_OFFSET_MAX; + baro_amsys_offset_init = TRUE; + } + pBaroRaw = 0; + baro_amsys_altitude = gps.hmsl / 1000.0; + baro_amsys_adc = baro_amsys_offset - ((baro_amsys_altitude - ground_alt) / INS_BARO_SENS); + baro_amsys_valid = TRUE; +#endif + +#ifdef BARO_AMSYS_SYNC_SEND + DOWNLINK_SEND_AMSYS_BARO(DefaultChannel, DefaultDevice, &pBaroRaw, &baro_amsys_p, &baro_amsys_offset, &ref_alt_init, &baro_amsys_abs_altitude, &baro_amsys_altitude, &baro_amsys_temp); +#else + RunOnceEvery(10, DOWNLINK_SEND_AMSYS_BARO(DefaultChannel, DefaultDevice, &pBaroRaw, &baro_amsys_p, &baro_amsys_offset, &ref_alt_init, &baro_amsys_abs_altitude, &baro_amsys_altitude, &baro_amsys_temp)); #endif } void baro_amsys_read_event( void ) { - pBaroRaw = 0; - // Get raw altimeter from buffer - pBaroRaw = (baro_amsys_i2c_trans.buf[0] << 8) | baro_amsys_i2c_trans.buf[1]; + pBaroRaw = 0; + // Get raw altimeter from buffer + pBaroRaw = (baro_amsys_i2c_trans.buf[0] << 8) | baro_amsys_i2c_trans.buf[1]; #ifdef MEASURE_AMSYS_TEMPERATURE - tBaroRaw = (baro_amsys_i2c_trans.buf[2] << 8) | baro_amsys_i2c_trans.buf[3]; - baro_amsys_temp = (float)(tBaroRaw-TEMPERATURE_AMSYS_OFFSET_MIN)*TEMPERATURE_AMSYS_MAX/(float)(TEMPERATURE_AMSYS_OFFSET_MAX-TEMPERATURE_AMSYS_OFFSET_MIN)+(float)TEMPERATURE_AMSYS_MIN; + tBaroRaw = (baro_amsys_i2c_trans.buf[2] << 8) | baro_amsys_i2c_trans.buf[3]; + baro_amsys_temp = (float)(tBaroRaw-TEMPERATURE_AMSYS_OFFSET_MIN)*TEMPERATURE_AMSYS_MAX/(float)(TEMPERATURE_AMSYS_OFFSET_MAX-TEMPERATURE_AMSYS_OFFSET_MIN)+(float)TEMPERATURE_AMSYS_MIN; #endif - // Check if this is valid altimeter - if (pBaroRaw == 0) - baro_amsys_valid = FALSE; - else - baro_amsys_valid = TRUE; + // Check if this is valid altimeter + if (pBaroRaw == 0) + baro_amsys_valid = FALSE; + else + baro_amsys_valid = TRUE; baro_amsys_adc = pBaroRaw; - // Continue only if a new altimeter value was received - //if (baro_amsys_valid && GpsFixValid()) { - if (baro_amsys_valid) { - //Cut RAW Min and Max - if (pBaroRaw < BARO_AMSYS_OFFSET_MIN) - pBaroRaw = BARO_AMSYS_OFFSET_MIN; - if (pBaroRaw > BARO_AMSYS_OFFSET_MAX) - pBaroRaw = BARO_AMSYS_OFFSET_MAX; + // Continue only if a new altimeter value was received + //if (baro_amsys_valid && GpsFixValid()) { + if (baro_amsys_valid) { + //Cut RAW Min and Max + if (pBaroRaw < BARO_AMSYS_OFFSET_MIN) + pBaroRaw = BARO_AMSYS_OFFSET_MIN; + if (pBaroRaw > BARO_AMSYS_OFFSET_MAX) + pBaroRaw = BARO_AMSYS_OFFSET_MAX; - //Convert to pressure - baro_amsys_p = (float)(pBaroRaw-BARO_AMSYS_OFFSET_MIN)*BARO_AMSYS_MAX_PRESSURE/(float)(BARO_AMSYS_OFFSET_MAX-BARO_AMSYS_OFFSET_MIN); - if (!baro_amsys_offset_init) { - --baro_amsys_cnt; - // Check if averaging completed - if (baro_amsys_cnt == 0) { - // Calculate average - baro_amsys_offset = (float)(baro_amsys_offset_tmp / BARO_AMSYS_OFFSET_NBSAMPLES_AVRG); - ref_alt_init = GROUND_ALT; - baro_amsys_offset_init = TRUE; + //Convert to pressure + baro_amsys_p = (float)(pBaroRaw-BARO_AMSYS_OFFSET_MIN)*BARO_AMSYS_MAX_PRESSURE/(float)(BARO_AMSYS_OFFSET_MAX-BARO_AMSYS_OFFSET_MIN); + if (!baro_amsys_offset_init) { + --baro_amsys_cnt; + // Check if averaging completed + if (baro_amsys_cnt == 0) { + // Calculate average + baro_amsys_offset = (float)(baro_amsys_offset_tmp / BARO_AMSYS_OFFSET_NBSAMPLES_AVRG); + ref_alt_init = GROUND_ALT; + baro_amsys_offset_init = TRUE; - // hight over Sea level at init point - //baro_amsys_offset_altitude = 288.15 / 0.0065 * (1 - pow((baro_amsys_p)/1013.25 , 1/5.255)); - } - // Check if averaging needs to continue - else if (baro_amsys_cnt <= BARO_AMSYS_OFFSET_NBSAMPLES_AVRG) - baro_amsys_offset_tmp += baro_amsys_p; + // hight over Sea level at init point + //baro_amsys_offset_altitude = 288.15 / 0.0065 * (1 - pow((baro_amsys_p)/1013.25 , 1/5.255)); + } + // Check if averaging needs to continue + else if (baro_amsys_cnt <= BARO_AMSYS_OFFSET_NBSAMPLES_AVRG) + baro_amsys_offset_tmp += baro_amsys_p; - baro_amsys_altitude = 0.0; + baro_amsys_altitude = 0.0; - } - else { - // Lowpassfiltering and convert pressure to altitude - baro_amsys_altitude = baro_filter * baro_old + (1 - baro_filter) * (baro_amsys_offset-baro_amsys_p)/(1.2041*9.81); - baro_old = baro_amsys_altitude; - //New value available - } - baro_amsys_abs_altitude=baro_amsys_altitude+ref_alt_init; - } /*else { - baro_amsys_abs_altitude = 0.0; - }*/ + } + else { + // Lowpassfiltering and convert pressure to altitude + baro_amsys_altitude = baro_filter * baro_old + (1 - baro_filter) * (baro_amsys_offset-baro_amsys_p)*baro_scale/(1.2041*9.81); + baro_old = baro_amsys_altitude; + //New value available + } + baro_amsys_abs_altitude=baro_amsys_altitude+ref_alt_init; + } /*else { + baro_amsys_abs_altitude = 0.0; + }*/ - // Transaction has been read - baro_amsys_i2c_trans.status = I2CTransDone; -#ifdef SENSOR_SYNC_SEND - DOWNLINK_SEND_AMSYS_BARO(DefaultChannel, DefaultDevice, &pBaroRaw, &baro_amsys_p, &baro_amsys_offset, &ref_alt_init, &baro_amsys_abs_altitude, &baro_amsys_altitude, &baro_amsys_temp) -#else - RunOnceEvery(10, DOWNLINK_SEND_AMSYS_BARO(DefaultChannel, DefaultDevice, &pBaroRaw, &baro_amsys_p, &baro_amsys_offset, &ref_alt_init, &baro_amsys_abs_altitude, &baro_amsys_altitude, &baro_amsys_temp)); -#endif - + // Transaction has been read + baro_amsys_i2c_trans.status = I2CTransDone; } diff --git a/sw/airborne/modules/sensors/baro_amsys.h b/sw/airborne/modules/sensors/baro_amsys.h index 7844695db6..a6852dab58 100644 --- a/sw/airborne/modules/sensors/baro_amsys.h +++ b/sw/airborne/modules/sensors/baro_amsys.h @@ -28,16 +28,16 @@ #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; extern bool_t baro_amsys_valid; -// extern bool_t baro_amsys_updated; -// extern bool_t baro_amsys_enabled; +extern bool_t baro_amsys_enabled; extern float baro_amsys_altitude; -// extern float baro_amsys_r; -// extern float baro_amsys_sigma2; +extern float baro_amsys_r; +extern float baro_amsys_sigma2; extern float baro_filter; extern struct i2c_transaction baro_amsys_i2c_trans; @@ -48,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 bdfce3a3af..5011b7d075 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 #ifndef DOWNLINK_DEVICE #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE @@ -54,196 +51,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.c b/sw/airborne/modules/sensors/baro_ets.c index 3f2e7fa1d6..e545e6d2a2 100644 --- a/sw/airborne/modules/sensors/baro_ets.c +++ b/sw/airborne/modules/sensors/baro_ets.c @@ -43,6 +43,7 @@ #include "mcu_periph/i2c.h" #include "state.h" #include +#include "mcu_periph/sys_time.h" #include "subsystems/nav.h" @@ -50,7 +51,7 @@ #include "subsystems/gps.h" #endif -#ifdef BARO_ETS_TELEMETRY +#ifdef BARO_ETS_SYNC_SEND #ifndef DOWNLINK_DEVICE #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE #endif @@ -58,7 +59,7 @@ #include "mcu_periph/uart.h" #include "messages.h" #include "subsystems/datalink/downlink.h" -#endif //BARO_ETS_TELEMETRY +#endif //BARO_ETS_SYNC_SEND #define BARO_ETS_ADDR 0xE8 #define BARO_ETS_REG 0x07 @@ -75,6 +76,13 @@ #ifndef BARO_ETS_I2C_DEV #define BARO_ETS_I2C_DEV i2c0 #endif +PRINT_CONFIG_VAR(BARO_ETS_I2C_DEV) + +/** delay in seconds until sensor is read after startup */ +#ifndef BARO_ETS_START_DELAY +#define BARO_ETS_START_DELAY 0.2 +#endif +PRINT_CONFIG_VAR(BARO_ETS_START_DELAY) // Global variables uint16_t baro_ets_adc; @@ -88,16 +96,18 @@ float baro_ets_sigma2; struct i2c_transaction baro_ets_i2c_trans; // Local variables -bool_t baro_ets_offset_init; +bool_t baro_ets_offset_init; uint32_t baro_ets_offset_tmp; uint16_t baro_ets_cnt; +uint32_t baro_ets_delay_time; +bool_t baro_ets_delay_done; void baro_ets_init( void ) { baro_ets_adc = 0; baro_ets_altitude = 0.0; baro_ets_offset = 0; baro_ets_offset_tmp = 0; - baro_ets_valid = TRUE; + baro_ets_valid = FALSE; baro_ets_offset_init = FALSE; baro_ets_enabled = TRUE; baro_ets_cnt = BARO_ETS_OFFSET_NBSAMPLES_INIT + BARO_ETS_OFFSET_NBSAMPLES_AVRG; @@ -105,15 +115,22 @@ void baro_ets_init( void ) { baro_ets_sigma2 = BARO_ETS_SIGMA2; baro_ets_i2c_trans.status = I2CTransDone; + + baro_ets_delay_done = FALSE; + SysTimeTimerStart(baro_ets_delay_time); } void baro_ets_read_periodic( void ) { // Initiate next read #ifndef SITL + if (!baro_ets_delay_done) { + if (SysTimeTimer(baro_ets_delay_time) < USEC_OF_SEC(BARO_ETS_START_DELAY)) return; + else baro_ets_delay_done = TRUE; + } if (baro_ets_i2c_trans.status == I2CTransDone) i2c_receive(&BARO_ETS_I2C_DEV, &baro_ets_i2c_trans, BARO_ETS_ADDR, 2); #else // SITL - /* fake an offset so sim works for under hmsl as well */ + /* fake an offset so sim works as well */ if (!baro_ets_offset_init) { baro_ets_offset = 12400; baro_ets_offset_init = TRUE; @@ -123,7 +140,7 @@ void baro_ets_read_periodic( void ) { baro_ets_valid = TRUE; #endif -#ifdef BARO_ETS_TELEMETRY +#ifdef BARO_ETS_SYNC_SEND DOWNLINK_SEND_BARO_ETS(DefaultChannel, DefaultDevice, &baro_ets_adc, &baro_ets_offset, &baro_ets_altitude); #endif } @@ -161,7 +178,7 @@ void baro_ets_read_event( void ) { if (baro_ets_offset_init) { baro_ets_altitude = ground_alt + BARO_ETS_SCALE * (float)(baro_ets_offset-baro_ets_adc); // New value available -#ifdef BARO_ETS_TELEMETRY +#ifdef BARO_ETS_SYNC_SEND DOWNLINK_SEND_BARO_ETS(DefaultChannel, DefaultDevice, &baro_ets_adc, &baro_ets_offset, &baro_ets_altitude); #endif } else { diff --git a/sw/airborne/modules/sensors/baro_ets.h b/sw/airborne/modules/sensors/baro_ets.h index ed6b7d0fe3..33f7bbd592 100644 --- a/sw/airborne/modules/sensors/baro_ets.h +++ b/sw/airborne/modules/sensors/baro_ets.h @@ -45,12 +45,12 @@ #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; extern bool_t baro_ets_valid; -extern bool_t baro_ets_updated; extern bool_t baro_ets_enabled; extern float baro_ets_altitude; extern float baro_ets_r; @@ -64,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 fd82d068eb..e44e3044f8 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 DOWNLINK_DEVICE #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE @@ -49,201 +47,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; 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, 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); - - 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 aeef952cd6..549b6bf731 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_i2c.h +++ b/sw/airborne/modules/sensors/baro_ms5611_i2c.h @@ -2,45 +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; -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; -void baro_ms5611_init(void); -void baro_ms5611_periodic(void); -void baro_ms5611_d1(void); -void baro_ms5611_d2(void); -void baro_ms5611_event(void); +extern struct Ms5611_I2c baro_ms5611; -#define BaroMs5611Update(_b) { if (baro_ms5611_valid) { _b = baro_ms5611_alt; baro_ms5611_valid = FALSE; } } +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_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/sd_card/main.c b/sw/airborne/sd_card/main.c deleted file mode 100644 index 37da825c60..0000000000 --- a/sw/airborne/sd_card/main.c +++ /dev/null @@ -1,91 +0,0 @@ -#include "std.h" -#include "mcu.h" -#include "mcu_periph/sys_time.h" -#include "led.h" -#include "mcu_periph/uart.h" - -#include "messages.h" -#include "subsystems/datalink/downlink.h" - -#include "subsystems/datalink/datalink.h" -#include "generated/settings.h" -#include "dl_protocol.h" - -#include "mcu_periph/spi.h" -#include "sd_card.h" - -static inline void main_init( void ); -static inline void main_periodic_task( void ); -static inline void main_event_task( void ); - - -uint8_t dl_buffer[MSG_SIZE] __attribute__ ((aligned)); -bool_t dl_msg_available; -uint16_t datalink_time; - -int main( void ) { - main_init(); - while(1) { - if (sys_time_check_and_ack_timer(0)) - main_periodic_task(); - main_event_task( ); - } - return 0; -} - -static inline void main_init( void ) { - mcu_init(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); - led_init(); - uart_init(&uart0); - - spi_init(); - sd_card_init(); - - mcu_int_enable(); -} - -static inline void main_periodic_task( void ) { - LED_TOGGLE(1); - //DOWNLINK_SEND_DEBUG(3,buf_input); -} - -static inline void main_event_task( void ) { - DatalinkEvent(); - - // spi event - if (spi_message_received) { - /* Got a message on SPI. */ - spi_message_received = FALSE; - sd_card_event(); - } - -} - - -#define IdOfMsg(x) (x[1]) - -void dl_parse_msg(void) { - - LED_TOGGLE(1); - - uint8_t msg_id = IdOfMsg(dl_buffer); - switch (msg_id) { - - case DL_PING: { - DOWNLINK_SEND_PONG(); - break; - } - - case DL_SETTING : { - uint8_t i = DL_SETTING_index(dl_buffer); - float var = DL_SETTING_value(dl_buffer); - DlSetting(i, var); - DOWNLINK_SEND_DL_VALUE(&i, &var); - break; - } - - } -} - - diff --git a/sw/airborne/sd_card/sd_card.c b/sw/airborne/sd_card/sd_card.c deleted file mode 100644 index 6ca1442682..0000000000 --- a/sw/airborne/sd_card/sd_card.c +++ /dev/null @@ -1,105 +0,0 @@ -/* - * Copyright (C) 2007 ENAC - * - * 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 - * \brief - * - */ - -#include "sd_card.h" -#include "spi.h" - -bool_t sd_card_available; - -static bool_t status_read_data; - -#define CMD_INIT_1 0x24 // set chanel AIN1/AIN2 and next operation on filter high -#define CMD_INIT_2 0xCF // set unipolar mode, 24 bits, no boost, filter high -#define CMD_INIT_3 0x34 // set chanel AIN1/AIN2 and next operation on filter low -#define CMD_INIT_4 0x00 // set low filter -#define CMD_INIT_5 0x14 // set chanel AIN1/AIN2 and next operation on mode register -#define CMD_INIT_6 0x20 // set gain to 1, burnout current off, no filter sync, self calibration -#define CMD_MEASUREMENT 0x54 // set chanel AIN1/AIN2 and next operation on data register - - -uint8_t buf_input[3]; -uint8_t buf_output[3]; - - -static void send1_on_spi(uint8_t d) { - buf_output[0] = d; - spi_buffer_length = 1; - - spi_buffer_input = (uint8_t*)&buf_input; - spi_buffer_output = (uint8_t*)&buf_output; - SpiStart(); -} - - -void sd_card_init( void ) { - - send1_on_spi(CMD_INIT_1); - send1_on_spi(CMD_INIT_2); - send1_on_spi(CMD_INIT_3); - send1_on_spi(CMD_INIT_4); - send1_on_spi(CMD_INIT_5); - send1_on_spi(CMD_INIT_6); - - status_read_data = FALSE; - sd_card_available = FALSE; - -} - -void sd_card_periodic(void) { - if (!SpiCheckAvailable()) { - SpiOverRun(); - return; - } - - if (status_read_data) { - buf_output[0] = buf_output[1] = buf_output[2] = 0; - spi_buffer_length = 3; - } - else { - buf_output[0] = CMD_MEASUREMENT; - spi_buffer_length = 1; - } - - spi_buffer_input = (uint8_t*)&buf_input; - spi_buffer_output = (uint8_t*)&buf_output; - //if (status_read_data) - // SpiSetCPHA(); - //else - // SpiClrCPHA(); - SpiStart(); -} - -/* Handle the SPI message, i.e. store the received values in variables */ -void sd_card_event( void ) { - - if (status_read_data) { - } /* else nothing to read */ - - status_read_data = !status_read_data; - -} - diff --git a/sw/airborne/state.c b/sw/airborne/state.c index 0b92f2fe3a..f72bc48438 100644 --- a/sw/airborne/state.c +++ b/sw/airborne/state.c @@ -69,8 +69,7 @@ void stateCalcPositionEcef_i(void) { ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f); } else if (bit_is_set(state.pos_status, POS_NED_I)) { - /// @todo check if resolution is good enough - ecef_of_ned_point_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i); + ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i); } else if (bit_is_set(state.pos_status, POS_NED_F)) { /* transform ned_f to ecef_f, set status bit, then convert to int */ @@ -114,7 +113,7 @@ void stateCalcPositionNed_i(void) { INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i); } else if (bit_is_set(state.pos_status, POS_ECEF_I)) { - ned_of_ecef_point_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i); + ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i); } else if (bit_is_set(state.pos_status, POS_ECEF_F)) { /* transform ecef_f -> ned_f, set status bit, then convert to int */ @@ -206,7 +205,7 @@ void stateCalcPositionEnu_i(void) { INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i); } else if (bit_is_set(state.pos_status, POS_ECEF_I)) { - enu_of_ecef_point_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i); + enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i); } else if (bit_is_set(state.pos_status, POS_ECEF_F)) { /* transform ecef_f -> enu_f, set status bit, then convert to int */ @@ -306,8 +305,7 @@ void stateCalcPositionLla_i(void) { } else if (bit_is_set(state.pos_status, POS_NED_I)) { /* transform ned_i -> ecef_i -> lla_i, set status bits */ - /// @todo check if resolution is enough - ecef_of_ned_point_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i); + ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i); SetBit(state.pos_status, POS_ECEF_I); lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i); /* uses double version internally */ } @@ -360,8 +358,7 @@ void stateCalcPositionEcef_f(void) { } else if (bit_is_set(state.pos_status, POS_NED_I)) { /* transform ned_i -> ecef_i -> ecef_f, set status bits */ - /// @todo check if resolution is good enough - ecef_of_ned_point_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i); + ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i); SetBit(state.pos_status, POS_ECEF_F); ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i); } @@ -396,7 +393,7 @@ void stateCalcPositionNed_f(void) { } else if (bit_is_set(state.pos_status, POS_ECEF_I)) { /* transform ecef_i -> ned_i -> ned_f, set status bits */ - ned_of_ecef_point_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i); + ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i); SetBit(state.pos_status, POS_NED_I); NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i); } @@ -407,7 +404,7 @@ void stateCalcPositionNed_f(void) { /* transform lla_i -> ecef_i -> ned_i -> ned_f, set status bits */ ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */ SetBit(state.pos_status, POS_ECEF_I); - ned_of_ecef_point_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i); + ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i); SetBit(state.pos_status, POS_NED_I); NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i); } @@ -481,7 +478,7 @@ void stateCalcPositionEnu_f(void) { } else if (bit_is_set(state.pos_status, POS_ECEF_I)) { /* transform ecef_i -> enu_i -> enu_f, set status bits */ - enu_of_ecef_point_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i); + enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i); SetBit(state.pos_status, POS_ENU_I); ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i); } @@ -492,7 +489,7 @@ void stateCalcPositionEnu_f(void) { /* transform lla_i -> ecef_i -> enu_i -> enu_f, set status bits */ ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */ SetBit(state.pos_status, POS_ECEF_I); - enu_of_ecef_point_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i); + enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i); SetBit(state.pos_status, POS_ENU_I); ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i); } 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/actuators/motor_mixing.c b/sw/airborne/subsystems/actuators/motor_mixing.c index 313ac9df90..e79daa50ed 100644 --- a/sw/airborne/subsystems/actuators/motor_mixing.c +++ b/sw/airborne/subsystems/actuators/motor_mixing.c @@ -172,13 +172,17 @@ void motor_mixing_run(bool_t motors_on, bool_t override_on, pprz_t in_cmd[] ) { if (min_cmd < MOTOR_MIXING_MIN_MOTOR && max_cmd > MOTOR_MIXING_MAX_MOTOR) motor_mixing.nb_failure++; - if (min_cmd < MOTOR_MIXING_MIN_MOTOR) { - int32_t saturation_offset = MOTOR_MIXING_MIN_MOTOR - min_cmd; + /* In case of both min and max saturation, only lower the throttle + * instead of applying both. This should prevent your quad shooting up, + * but it might loose altitude in case of such a saturation failure. + */ + if (max_cmd > MOTOR_MIXING_MAX_MOTOR) { + int32_t saturation_offset = MOTOR_MIXING_MAX_MOTOR - max_cmd; BoundAbs(saturation_offset, MOTOR_MIXING_MAX_SATURATION_OFFSET); offset_commands(saturation_offset); } - if (max_cmd > MOTOR_MIXING_MAX_MOTOR) { - int32_t saturation_offset = MOTOR_MIXING_MAX_MOTOR - max_cmd; + else if (min_cmd < MOTOR_MIXING_MIN_MOTOR) { + int32_t saturation_offset = MOTOR_MIXING_MIN_MOTOR - min_cmd; BoundAbs(saturation_offset, MOTOR_MIXING_MAX_SATURATION_OFFSET); offset_commands(saturation_offset); } diff --git a/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c b/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c index afed7a6202..1e5317e937 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" @@ -55,15 +60,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; @@ -78,6 +110,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 @@ -112,6 +147,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 @@ -123,7 +161,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 0a17453b3a..5db9eca134 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c @@ -107,6 +107,7 @@ 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); } void ahrs_align(void) { @@ -240,9 +241,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); @@ -263,8 +263,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; @@ -273,7 +271,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_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c index b53f0bdea0..0c656ce4a4 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c @@ -20,7 +20,6 @@ * Theory: http://code.google.com/p/gentlenav/downloads/list file DCMDraft2.pdf * * Options: - * - USE_HIGH_ACCEL_FLAG: no compensation when high accelerations present * - USE_MAGNETOMETER_ONGROUND: use magnetic compensation before takeoff only while GPS course not good * - USE_AHRS_GPS_ACCELERATIONS: forward acceleration compensation from GPS speed * @@ -114,18 +113,6 @@ int renorm_blowup_count = 0; float imu_health = 0.; #endif -#if USE_HIGH_ACCEL_FLAG -// High Accel Flag -#define HIGH_ACCEL_LOW_SPEED 15.0 -#define HIGH_ACCEL_LOW_SPEED_RESUME 4.0 // Hysteresis -#define HIGH_ACCEL_HIGH_THRUST (0.8*MAX_PPRZ) -#define HIGH_ACCEL_HIGH_THRUST_RESUME (0.1*MAX_PPRZ) // Hysteresis -bool_t high_accel_done; -bool_t high_accel_flag; -// Command vector for thrust (fixed_wing) -#include "inter_mcu.h" -#endif - static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat) { @@ -155,11 +142,6 @@ void ahrs_init(void) { /* set inital filter dcm */ set_dcm_matrix_from_rmat(&ahrs_impl.body_to_imu_rmat); -#if USE_HIGH_ACCEL_FLAG - high_accel_done = FALSE; - high_accel_flag = FALSE; -#endif - ahrs_impl.gps_speed = 0; ahrs_impl.gps_acceleration = 0; ahrs_impl.gps_course = 0; @@ -437,25 +419,6 @@ void Drift_correction(void) // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0) Accel_weight = Chop(1 - 2*fabs(1 - Accel_magnitude),0,1); // -#if USE_HIGH_ACCEL_FLAG - // Test for high acceleration: - // - low speed - // - high thrust - float speed = *stateGetHorizontalSpeedNorm_f(); - if (speed < HIGH_ACCEL_LOW_SPEED && ap_state->commands[COMMAND_THROTTLE] > HIGH_ACCEL_HIGH_THRUST && !high_accel_done) { - high_accel_flag = TRUE; - } else { - high_accel_flag = FALSE; - if (speed > HIGH_ACCEL_LOW_SPEED && !high_accel_done) { - high_accel_done = TRUE; // After takeoff, don't use high accel before landing (GS small, Throttle small) - } - if (speed < HIGH_ACCEL_HIGH_THRUST_RESUME && ap_state->commands[COMMAND_THROTTLE] < HIGH_ACCEL_HIGH_THRUST_RESUME) { - high_accel_done = FALSE; // Activate high accel after landing - } - } - if (high_accel_flag) { Accel_weight = 0.; } -#endif - #if PERFORMANCE_REPORTING == 1 { 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 e3e38086dc..0000000000 --- a/sw/airborne/subsystems/ahrs/ahrs_float_lkf.c +++ /dev/null @@ -1,857 +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); \ - } - - -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); - -} - -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..4fa28e6f48 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 39883053b6..851414b494 100644 --- a/sw/airborne/subsystems/datalink/downlink.h +++ b/sw/airborne/subsystems/datalink/downlink.h @@ -46,16 +46,21 @@ #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 #ifdef USE_USB_SERIAL #include "mcu_periph/usb_serial.h" #endif + #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..fc8efbb801 --- /dev/null +++ b/sw/airborne/subsystems/datalink/superbitrf.c @@ -0,0 +1,978 @@ +/* + * 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 }; + +/** + * 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); +} + +/** + * 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 7ed3b06cb5..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,12 +86,15 @@ 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 /* measure current if available, otherwise estimate it */ -#if defined ADC_CHANNEL_CURRENT +#if defined ADC_CHANNEL_CURRENT && !defined SITL adc_buf_channel(ADC_CHANNEL_CURRENT, &electrical_priv.current_adc_buf, DEFAULT_AV_NB_SAMPLE); #elif defined MILLIAMP_AT_FULL_THROTTLE PRINT_CONFIG_VAR(CURRENT_ESTIMATION_NONLINEARITY) @@ -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 60a1e5db78..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" @@ -36,6 +40,11 @@ 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); @@ -51,6 +60,12 @@ void gps_ardrone2_parse(navdata_gps_t *navdata_gps) { LLA_BFP_OF_REAL(gps.lla_pos, gps_lla_d); // TODO: parse other stuff + gps.nb_channels = GPS_NB_CHANNELS; + + for(i = 0; i < GPS_NB_CHANNELS; i++) { + gps.svinfos[i].svid = navdata_gps->channels[i].sat; + gps.svinfos[i].cno = navdata_gps->channels[i].cn0; + } // Check if we have a fix TODO: check if 2D or 3D fix? if (navdata_gps->gps_state == 1) diff --git a/sw/airborne/subsystems/gps/gps_ardrone2.h b/sw/airborne/subsystems/gps/gps_ardrone2.h index 42e926ff34..2995d0f853 100644 --- a/sw/airborne/subsystems/gps/gps_ardrone2.h +++ b/sw/airborne/subsystems/gps/gps_ardrone2.h @@ -30,7 +30,7 @@ #include "boards/ardrone/at_com.h" -//#define GPS_NB_CHANNELS 12 // TODO: Get channels out of packet +#define GPS_NB_CHANNELS 12 extern bool_t gps_ardrone2_available; /* 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 74432019cb..3d9998bb99 100644 --- a/sw/airborne/subsystems/imu.c +++ b/sw/airborne/subsystems/imu.c @@ -27,6 +27,7 @@ #include "subsystems/imu.h" struct Imu imu; +struct ImuFloat imuf; void imu_init(void) { @@ -60,16 +61,14 @@ INFO("Magnetometer neutrals are set to zero, you should calibrate!") } -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_aspirin2.c b/sw/airborne/subsystems/imu/imu_aspirin2.c deleted file mode 100644 index b467e1bd35..0000000000 --- a/sw/airborne/subsystems/imu/imu_aspirin2.c +++ /dev/null @@ -1,346 +0,0 @@ -/* - * Copyright (C) 2012 Christophe DeWagter - * - * 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 "subsystems/imu.h" - -#include "led.h" -#include "mcu_periph/spi.h" - -// Peripherials -#include "peripherals/mpu60x0_regs.h" -#include "peripherals/hmc58xx_regs.h" -#include "peripherals/ms5611.h" - -#ifndef MPU6000_SLAVE_IDX -#define MPU6000_SLAVE_IDX SPI_SLAVE2 -#endif - -#ifndef MPU6000_SPI_DEV -#define MPU6000_SPI_DEV spi2 -#endif - -/* HMC58XX default conf */ -#ifndef HMC58XX_DO -#define HMC58XX_DO 0x6 // Data Output Rate (6 -> 50Hz with HMC5843, 75Hz with HMC5883) -#endif -#ifndef HMC58XX_MS -#define HMC58XX_MS 0x0 // Measurement configuration -#endif -#ifndef HMC58XX_GN -#define HMC58XX_GN 0x1 // Gain configuration (1 -> +- 1 Gauss) -#endif -#ifndef HMC58XX_MD -#define HMC58XX_MD 0x0 // Continious measurement mode -#endif - -#define HMC58XX_CRA ((HMC58XX_DO<<2)|(HMC58XX_MS)) -#define HMC58XX_CRB (HMC58XX_GN<<5) - -struct ImuAspirin2 imu_aspirin2; - -struct spi_transaction aspirin2_mpu60x0; - -// initialize peripherals -static void mpu_configure(void); -static void trans_cb( struct spi_transaction *trans ); - -void imu_impl_init(void) { - aspirin2_mpu60x0.select = SPISelectUnselect; - aspirin2_mpu60x0.cpol = SPICpolIdleHigh; - aspirin2_mpu60x0.cpha = SPICphaEdge2; - aspirin2_mpu60x0.dss = SPIDss8bit; - aspirin2_mpu60x0.bitorder = SPIMSBFirst; - aspirin2_mpu60x0.cdiv = SPIDiv64; - aspirin2_mpu60x0.slave_idx = MPU6000_SLAVE_IDX; - aspirin2_mpu60x0.output_length = IMU_ASPIRIN_BUFFER_LEN; - aspirin2_mpu60x0.input_length = IMU_ASPIRIN_BUFFER_LEN; - aspirin2_mpu60x0.after_cb = trans_cb; - - imu_aspirin2.status = Aspirin2StatusUninit; - imu_aspirin2.imu_available = FALSE; - aspirin2_mpu60x0.input_buf = &imu_aspirin2.input_buf_p[0]; - aspirin2_mpu60x0.output_buf = &imu_aspirin2.output_buf_p[0]; -} - - -void imu_periodic(void) -{ - - if (imu_aspirin2.status == Aspirin2StatusUninit) { - mpu_configure(); - imu_aspirin2.status = Aspirin2StatusIdle; - - aspirin2_mpu60x0.output_length = 22; - aspirin2_mpu60x0.input_length = 22; - aspirin2_mpu60x0.output_buf[0] = MPU60X0_REG_INT_STATUS + MPU60X0_SPI_READ; - for (int i=1; i6) - (MPU_DIG_FILTER << 0) ); // Low-Pass Filter - - // MPU60X0_REG_SMPLRT_DIV - mpu_set( MPU60X0_REG_SMPLRT_DIV, MPU_SMPLRT_DIV); - - // MPU60X0_REG_GYRO_CONFIG - mpu_set( MPU60X0_REG_GYRO_CONFIG, - (3 << 3) ); // -2000deg/sec - - // MPU60X0_REG_ACCEL_CONFIG - mpu_set( MPU60X0_REG_ACCEL_CONFIG, - (0 << 0) | // No HPFL - (3 << 3) ); // Full Scale = 16g - -#ifndef MPU6000_NO_SLAVES -PRINT_CONFIG_MSG("Reading MPU slaves") - - ///////////////////////////////////// - // SPI Slave Configuration Section - - // Power the Aux I2C Circuit: - // MPU60X0_REG_AUX_VDDIO = 0 (good on startup): (0 << 7); // MPU6000: 0=Vdd. MPU6050 : 0=VLogic 1=Vdd - - // MPU60X0_REG_USER_CTRL: - mpu_set( MPU60X0_REG_USER_CTRL, - (1 << 5) | // I2C_MST_EN: Enable Aux I2C Master Mode - (1 << 4) | // I2C_IF_DIS: Disable I2C on primary interface - (0 << 1) ); // Trigger a I2C_MST_RESET - - // Enable the aux i2c - mpu_set( MPU60X0_REG_I2C_MST_CTRL, - (0 << 7) | // no multimaster - (0 << 6) | // do not delay IRQ waiting for all external slaves - (0 << 5) | // no slave 3 FIFO - (0 << 4) | // restart or stop/start from one slave to another: read -> write is always stop/start - (8 << 0) ); // 0=348kHz 8=256kHz, 9=500kHz - - mpu_set( MPU60X0_REG_I2C_MST_DELAY, - (0 << 2) | // No Delay Slave 2 - (1 << 3) ); // Delay Slave 3 - -#if defined IMU_ASPIRIN_VERSION_2_1 && USE_IMU_ASPIRIN2_BARO_SLAVE - - // MS5611 Send Reset - mpu_set( MPU60X0_REG_I2C_SLV4_ADDR, (MS5611_ADDR0)); - mpu_set( MPU60X0_REG_I2C_SLV4_DO, MS5611_REG_RESET); - mpu_set( MPU60X0_REG_I2C_SLV4_CTRL, - (1 << 7) | // Slave 4 enable - (0 << 6) | // Byte Swap - (1 << 5) | // Reg_Dis: do not write the register, just the data - (0 << 0) ); // Full Speed - - mpu_wait_slave4_ready(); - - // Wait at least 2.8ms - -#endif // read MS5611 as MPU slave - - // HMC5883 Magnetometer Configuration - - mpu_set( MPU60X0_REG_I2C_SLV4_ADDR, (HMC58XX_ADDR >> 1)); - mpu_set( MPU60X0_REG_I2C_SLV4_REG, HMC58XX_REG_CFGA); - mpu_set( MPU60X0_REG_I2C_SLV4_DO, HMC58XX_CRA); - mpu_set( MPU60X0_REG_I2C_SLV4_CTRL, - (1 << 7) | // Slave 4 enable - (0 << 6) | // Byte Swap - (0 << 0) ); // Full Speed - - mpu_wait_slave4_ready(); - - mpu_set( MPU60X0_REG_I2C_SLV4_ADDR, (HMC58XX_ADDR >> 1)); - mpu_set( MPU60X0_REG_I2C_SLV4_REG, HMC58XX_REG_CFGB); - mpu_set( MPU60X0_REG_I2C_SLV4_DO, HMC58XX_CRB); - mpu_set( MPU60X0_REG_I2C_SLV4_CTRL, - (1 << 7) | // Slave 4 enable - (0 << 6) | // Byte Swap - (0 << 0) ); // Full Speed - - mpu_wait_slave4_ready(); - - mpu_set( MPU60X0_REG_I2C_SLV4_ADDR, (HMC58XX_ADDR >> 1)); - mpu_set( MPU60X0_REG_I2C_SLV4_REG, HMC58XX_REG_MODE); - mpu_set( MPU60X0_REG_I2C_SLV4_DO, HMC58XX_MD); - mpu_set( MPU60X0_REG_I2C_SLV4_CTRL, - (1 << 7) | // Slave 4 enable - (0 << 6) | // Byte Swap - (3 << 0) ); // From now on a delayed rate of 1/4 is defined... - - // HMC5883 Reading: - // a) write hmc-register to HMC - // b) read 6 bytes from HMC - - mpu_set( MPU60X0_REG_I2C_SLV0_ADDR, (HMC58XX_ADDR >> 1) | MPU60X0_SPI_READ); - mpu_set( MPU60X0_REG_I2C_SLV0_REG, HMC58XX_REG_DATXM); - // Put the enable command as last. - mpu_set( MPU60X0_REG_I2C_SLV0_CTRL, - (1 << 7) | // Slave 0 enable - (0 << 6) | // Byte Swap - (6 << 0) ); // Read 6 bytes - - // Slave 0 Control: - -#if defined IMU_ASPIRIN_VERSION_2_1 && USE_IMU_ASPIRIN2_BARO_SLAVE -PRINT_CONFIG_MSG("Reading the MS5611 as MPU slave") -/* - - - // Read MS5611 Calibration - mpu_set( MPU60X0_REG_I2C_SLV1_ADDR, (MS5611_ADDR0) | MPU60X0_SPI_READ); - mpu_set( MPU60X0_REG_I2C_SLV1_REG, MS5611_REG_ADCREAD); - // Put the enable command as last. - mpu_set( MPU60X0_REG_I2C_SLV1_CTRL, - (1 << 7) | // Slave 1 enable - (0 << 6) | // Byte Swap - (3 << 0) ); // Read 6 bytes - -*/ - - // Full Rate Request For Pressure - mpu_set( MPU60X0_REG_I2C_SLV2_ADDR, (MS5611_ADDR0)); - mpu_set( MPU60X0_REG_I2C_SLV2_DO, 0x48); - // Put the enable command as last. - mpu_set( MPU60X0_REG_I2C_SLV2_CTRL, - (1 << 7) | // Slave 2 enable - (0 << 6) | // Byte Swap - (1 << 5) | // Rig Dis: Write Only - (1 << 0) ); // Write 1 byte - - // Reduced rate request For Temperature: Overwrites the Pressure Request - mpu_set( MPU60X0_REG_I2C_SLV3_ADDR, (MS5611_ADDR0)); - mpu_set( MPU60X0_REG_I2C_SLV3_DO, 0x58); - // Put the enable command as last. - mpu_set( MPU60X0_REG_I2C_SLV3_CTRL, - (1 << 7) | // Slave 2 enable - (0 << 6) | // Byte Swap - (1 << 5) | // Rig Dis: Write Only - (1 << 0) ); // Write 1 byte - - mpu_set( MPU60X0_REG_I2C_SLV1_ADDR, (MS5611_ADDR0) | MPU60X0_SPI_READ); - mpu_set( MPU60X0_REG_I2C_SLV1_REG, MS5611_REG_ADCREAD); - // Put the enable command as last. - mpu_set( MPU60X0_REG_I2C_SLV1_CTRL, - (1 << 7) | // Slave 1 enable - (0 << 6) | // Byte Swap - (3 << 0) ); // Read 6 bytes - -#endif // read MS5611 as MPU slave - -#endif - -} - diff --git a/sw/airborne/subsystems/imu/imu_aspirin2.h b/sw/airborne/subsystems/imu/imu_aspirin2.h deleted file mode 100644 index b55872f453..0000000000 --- a/sw/airborne/subsystems/imu/imu_aspirin2.h +++ /dev/null @@ -1,167 +0,0 @@ -/* - * Copyright (C) 2012 Christophe DeWagter - * - * 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 IMU_ASPIRIN_2_H -#define IMU_ASPIRIN_2_H - -#include "generated/airframe.h" -#include "subsystems/imu.h" - - -#if defined IMU_ASPIRIN_VERSION_2_1 || defined IMU_ASPIRIN_VERSION_2_2 -#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 -#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 - * MPU60X0 has 16.4 LSB/(deg/s) at 2000deg/s range - * sens = 1/16.4 * pi/180 * 2^INT32_RATE_FRAC - * sens = 1/16.4 * pi/180 * 4096 = 4.359066229 - */ -#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 - * MPU60X0 has 2048 LSB/g - * fixed point sens: 9.81 [m/s^2] / 2048 [LSB/g] * 2^INT32_ACCEL_FRAC - * sens = 9.81 / 2048 * 1024 = 4.905 - */ -#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS -#define IMU_ACCEL_X_SENS 4.905 -#define IMU_ACCEL_X_SENS_NUM 4905 -#define IMU_ACCEL_X_SENS_DEN 1000 -#define IMU_ACCEL_Y_SENS 4.905 -#define IMU_ACCEL_Y_SENS_NUM 4905 -#define IMU_ACCEL_Y_SENS_DEN 1000 -#define IMU_ACCEL_Z_SENS 4.905 -#define IMU_ACCEL_Z_SENS_NUM 4905 -#define IMU_ACCEL_Z_SENS_DEN 1000 -#endif -#if !defined IMU_ACCEL_X_NEUTRAL & !defined IMU_ACCEL_Y_NEUTRAL & !defined IMU_ACCEL_Z_NEUTRAL -#define IMU_ACCEL_X_NEUTRAL 0 -#define IMU_ACCEL_Y_NEUTRAL 0 -#define IMU_ACCEL_Z_NEUTRAL 0 -#endif - - -enum Aspirin2Status - { Aspirin2StatusUninit, - Aspirin2StatusIdle, - Aspirin2StatusReading - }; - -#define IMU_ASPIRIN_BUFFER_LEN 32 - -struct ImuAspirin2 { - volatile enum Aspirin2Status status; - volatile uint8_t imu_available; - volatile uint8_t input_buf_p[IMU_ASPIRIN_BUFFER_LEN]; - volatile uint8_t output_buf_p[IMU_ASPIRIN_BUFFER_LEN]; -}; - -extern struct ImuAspirin2 imu_aspirin2; - - -static inline int imu_from_buff(volatile uint8_t *buf) -{ - int32_t x, y, z, p, q, r, Mx, My, Mz; - -#define MPU_OFFSET_STATUS 1 - if (!(buf[MPU_OFFSET_STATUS] & 0x01)) { - return 0; - } - -#define MPU_OFFSET_GYRO 10 - p = (int16_t) ((buf[0+MPU_OFFSET_GYRO] << 8) | buf[1+MPU_OFFSET_GYRO]); - q = (int16_t) ((buf[2+MPU_OFFSET_GYRO] << 8) | buf[3+MPU_OFFSET_GYRO]); - r = (int16_t) ((buf[4+MPU_OFFSET_GYRO] << 8) | buf[5+MPU_OFFSET_GYRO]); - -#define MPU_OFFSET_ACC 2 - x = (int16_t) ((buf[0+MPU_OFFSET_ACC] << 8) | buf[1+MPU_OFFSET_ACC]); - y = (int16_t) ((buf[2+MPU_OFFSET_ACC] << 8) | buf[3+MPU_OFFSET_ACC]); - z = (int16_t) ((buf[4+MPU_OFFSET_ACC] << 8) | buf[5+MPU_OFFSET_ACC]); - -#define MPU_OFFSET_MAG 16 - Mx = (int16_t) ((buf[0+MPU_OFFSET_MAG] << 8) | buf[1+MPU_OFFSET_MAG]); - My = (int16_t) ((buf[2+MPU_OFFSET_MAG] << 8) | buf[3+MPU_OFFSET_MAG]); - Mz = (int16_t) ((buf[4+MPU_OFFSET_MAG] << 8) | buf[5+MPU_OFFSET_MAG]); - -#ifdef LISA_M_LONGITUDINAL_X - RATES_ASSIGN(imu.gyro_unscaled, q, -p, r); - VECT3_ASSIGN(imu.accel_unscaled, y, -x, z); - VECT3_ASSIGN(imu.mag_unscaled, -Mx, -Mz, My); -#else - RATES_ASSIGN(imu.gyro_unscaled, p, q, r); - VECT3_ASSIGN(imu.accel_unscaled, x, y, z); - VECT3_ASSIGN(imu.mag_unscaled, Mz, -Mx, My); -#endif - - return 1; -} - - -static inline void imu_aspirin2_event(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) -{ - if (imu_aspirin2.status == Aspirin2StatusUninit) return; - - if (imu_aspirin2.imu_available) { - imu_aspirin2.imu_available = FALSE; - if (imu_from_buff(imu_aspirin2.input_buf_p)) { - _gyro_handler(); - _accel_handler(); - _mag_handler(); - } - } -} - -#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \ - imu_aspirin2_event(_gyro_handler, _accel_handler, _mag_handler); \ -} - -#endif /* IMU_ASPIRIN_2_H */ diff --git a/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c index 84918649e4..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) @@ -145,10 +159,11 @@ void imu_aspirin2_event(void) { mpu60x0_spi_event(&imu_aspirin2.mpu); if (imu_aspirin2.mpu.data_available) { + /* HMC5883 has xzy order of axes in returned data */ struct Int32Vect3 mag; mag.x = Int16FromBuf(imu_aspirin2.mpu.data_ext, 0); - mag.y = Int16FromBuf(imu_aspirin2.mpu.data_ext, 2); - mag.z = Int16FromBuf(imu_aspirin2.mpu.data_ext, 4); + mag.z = Int16FromBuf(imu_aspirin2.mpu.data_ext, 2); + mag.y = Int16FromBuf(imu_aspirin2.mpu.data_ext, 4); #ifdef LISA_M_LONGITUDINAL_X RATES_ASSIGN(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates.q, @@ -158,11 +173,29 @@ void imu_aspirin2_event(void) imu_aspirin2.mpu.data_accel.vect.y, -imu_aspirin2.mpu.data_accel.vect.x, imu_aspirin2.mpu.data_accel.vect.z); - VECT3_ASSIGN(imu.mag_unscaled, -mag.x, -mag.z, mag.y); + 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_ASSIGN(imu.mag_unscaled, mag.z, -mag.x, mag.y) + 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 8b673b1ea0..8f0004ffd1 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); @@ -192,6 +196,12 @@ void alt_kalman(float z_meas) { R = baro_ms5611_r; SIGMA2 = baro_ms5611_sigma2; } else +#elif USE_BARO_AMSYS + if (baro_amsys_enabled) { + DT = BARO_AMSYS_DT; + R = baro_amsys_r; + SIGMA2 = baro_amsys_sigma2; + } else #elif USE_BARO_BMP if (baro_bmp_enabled) { DT = BARO_BMP_DT; diff --git a/sw/airborne/subsystems/ins/ins_alt_float.h b/sw/airborne/subsystems/ins/ins_alt_float.h index a3bf0ef210..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 @@ -52,6 +52,10 @@ #include "modules/sensors/baro_ms5611_i2c.h" #endif +#if USE_BARO_AMSYS +#include "modules/sensors/baro_amsys.h" +#endif + extern int32_t ins_qfe; extern float ins_baro_alt; extern bool_t ins_baro_initialised; diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c index 3dd2985273..950397b7fa 100644 --- a/sw/airborne/subsystems/ins/ins_int.c +++ b/sw/airborne/subsystems/ins/ins_int.c @@ -73,10 +73,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 */ @@ -107,9 +103,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; @@ -185,9 +178,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); @@ -263,13 +253,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 48d96f0b09..307c253f05 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 54bf0c612b..9c8cf7300f 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 new file mode 100644 index 0000000000..184ad03400 --- /dev/null +++ b/sw/airborne/subsystems/radio_control/sbus.c @@ -0,0 +1,145 @@ +/* + * Copyright (C) 2013 Alexandre Bustico, Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** @file subsystems/radio_control/sbus.c + * + * Futaba SBUS decoder + */ + +#include "subsystems/radio_control.h" +#include "subsystems/radio_control/sbus.h" +#include BOARD_CONFIG +#include "mcu_periph/uart.h" +#include "mcu_periph/gpio.h" +#include + +/* + * SBUS protocol and state machine status + */ +#define SBUS_START_BYTE 0x0f +#define SBUS_END_BYTE 0x00 +#define SBUS_BIT_PER_CHANNEL 11 +#define SBUS_BIT_PER_BYTE 8 +#define SBUS_FLAGS_BYTE 22 +#define SBUS_FRAME_LOST_BIT 2 + +#define SBUS_STATUS_UNINIT 0 +#define SBUS_STATUS_GOT_START 1 + +/** Set polarity using RC_POLARITY_GPIO. + * SBUS signal has a reversed polarity compared to normal UART + * this allows to using hardware UART peripheral by changing + * the input signal polarity. + * Setting this gpio ouput high inverts the signal, + * output low sets it to normal polarity. + */ +#ifndef RC_SET_POLARITY +#define RC_SET_POLARITY gpio_set +#endif + + +/** SBUS struct */ +struct _sbus sbus; + +// Init function +void radio_control_impl_init(void) { + sbus.frame_available = FALSE; + sbus.status = SBUS_STATUS_UNINIT; + + // Set UART parameters (100K, 8 bits, 2 stops, even parity) + uart_periph_set_bits_stop_parity(&SBUS_UART_DEV, UBITS_8, USTOP_2, UPARITY_EVEN); + uart_periph_set_baudrate(&SBUS_UART_DEV, B100000); + + // Set polarity +#ifdef RC_POLARITY_GPIO_PORT + gpio_setup_output(RC_POLARITY_GPIO_PORT, RC_POLARITY_GPIO_PIN); + RC_SET_POLARITY(RC_POLARITY_GPIO_PORT, RC_POLARITY_GPIO_PIN); +#endif +} + + +/** Decode the raw buffer */ +static void decode_sbus_buffer (const uint8_t *src, uint16_t *dst, bool_t *available) +{ + // reset counters + uint8_t byteInRawBuf = 0; + uint8_t bitInRawBuf = 0; + uint8_t channel = 0; + uint8_t bitInChannel = 0; + + // clear bits + memset (dst, 0, SBUS_NB_CHANNEL*sizeof(uint16_t)); + + // decode sbus data + for (uint8_t i=0; i< (SBUS_NB_CHANNEL*SBUS_BIT_PER_CHANNEL); i++) { + if (src[byteInRawBuf] & (1< 0) { \ + radio_control.radio_ok_cpt--; \ + } else { \ + radio_control.status = RC_OK; \ + NormalizePpmIIR(sbus.pulses,radio_control); \ + _received_frame_handler(); \ + } \ + sbus.frame_available = FALSE; \ + } \ +} + +#endif /* RC_SBUS_H */ 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/Makefile b/sw/airborne/test/Makefile index f8e05d1151..5938ed851a 100644 --- a/sw/airborne/test/Makefile +++ b/sw/airborne/test/Makefile @@ -4,7 +4,8 @@ Q=@ CC = gcc -CFLAGS = -std=c99 -I.. -I../../include -I../booz -I../../booz -Wall +CFLAGS = -std=c99 -I.. -I../../include -Wall +#CFLAGS += -DDEBUG LDFLAGS = -lm 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/airborne/test/test_geodetic.c b/sw/airborne/test/test_geodetic.c index e4c24a983e..51197828f5 100644 --- a/sw/airborne/test/test_geodetic.c +++ b/sw/airborne/test/test_geodetic.c @@ -20,6 +20,7 @@ static void test_lla_of_utm(void); static void test_floats(void); static void test_doubles(void); static void test_enu_of_ecef_int(void); +static void test_ecef_of_ned_int(void); static void test_ned_to_ecef_to_ned(void); static void test_enu_to_ecef_to_enu( void ); @@ -32,12 +33,16 @@ int main(int argc, char** argv) { //test_floats(); //test_doubles(); - test_lla_of_utm(); + //test_lla_of_utm(); + + //test_ned_to_ecef_to_ned(); + + //test_enu_to_ecef_to_enu(); + + test_ecef_of_ned_int(); //test_enu_of_ecef_int(); - // test_ned_to_ecef_to_ned(); - // test_enu_to_ecef_to_enu(); return 0; } @@ -48,7 +53,7 @@ static void test_lla_of_utm(void) { struct UtmCoor_d utm_d = { .east=348805.71, .north=4759354.89, .zone=31 }; struct LlaCoor_d lla_d; struct LlaCoor_d l_ref_d = {.lat=0.749999999392454875, - .lon=0.019999999054505127}; + .lon=0.019999999054505127}; lla_of_utm_d(&lla_d, &utm_d); printf(" lat=%.16f lon=%.16f\nref_lat=%.16f ref_lon=%.16f\n", lla_d.lat, lla_d.lon, l_ref_d.lat, l_ref_d.lon); @@ -58,7 +63,7 @@ static void test_lla_of_utm(void) { struct UtmCoor_f utm_f = { .east=348805.71, .north=4759354.89, .zone=31 }; struct LlaCoor_f lla_f; struct LlaCoor_f l_ref_f = {.lat=0.749999999392454875, - .lon=0.019999999054505127}; + .lon=0.019999999054505127}; lla_of_utm_f(&lla_f, &utm_f); printf(" lat=%.16f lon=%.16f\nref_lat=%.16f ref_lon=%.16f\n", lla_f.lat, lla_f.lon, l_ref_f.lat, l_ref_f.lon); @@ -78,15 +83,16 @@ static void test_floats(void) { struct LtpDef_f ltp_def; ltp_def_from_ecef_f(<p_def, &ref_coor); - printf("lla0 : (%f,%f,%f)\n", DegOfRad(ltp_def.lla.lat), DegOfRad(ltp_def.lla.lon), ltp_def.lla.alt); + printf("lla0 : (%f,%f,%f)\n", DegOfRad(ltp_def.lla.lat), DegOfRad(ltp_def.lla.lon), + ltp_def.lla.alt); struct EcefCoor_f my_ecef_point = ref_coor; struct EnuCoor_f my_enu_point; enu_of_ecef_point_f(&my_enu_point, <p_def, &my_ecef_point); printf("ecef to enu : (%f,%f,%f) -> (%f,%f,%f)\n", - my_ecef_point.x, my_ecef_point.y, my_ecef_point.z, - my_enu_point.x, my_enu_point.y, my_enu_point.z ); + my_ecef_point.x, my_ecef_point.y, my_ecef_point.z, + my_enu_point.x, my_enu_point.y, my_enu_point.z ); printf("\n"); } @@ -105,18 +111,91 @@ static void test_doubles(void) { struct LtpDef_d ltp_def; ltp_def_from_ecef_d(<p_def, &ref_coor); - printf("lla0 : (%f,%f,%f)\n", DegOfRad(ltp_def.lla.lat), DegOfRad(ltp_def.lla.lon), ltp_def.lla.alt); + printf("lla0 : (%f,%f,%f)\n", DegOfRad(ltp_def.lla.lat), DegOfRad(ltp_def.lla.lon), + ltp_def.lla.alt); struct EcefCoor_d my_ecef_point = ref_coor; struct EnuCoor_d my_enu_point; enu_of_ecef_point_d(&my_enu_point, <p_def, &my_ecef_point); printf("ecef to enu : (%f,%f,%f) -> (%f,%f,%f)\n", - my_ecef_point.x, my_ecef_point.y, my_ecef_point.z, - my_enu_point.x, my_enu_point.y, my_enu_point.z ); + my_ecef_point.x, my_ecef_point.y, my_ecef_point.z, + my_enu_point.x, my_enu_point.y, my_enu_point.z ); printf("\n"); } +static void test_ecef_of_ned_int(void) { + + printf("\n--- ecef_of_ned int ---\n"); + struct EcefCoor_d ref_coor_d = { 4624497.0 , 116475.0, 4376563.0}; + struct LtpDef_d ltp_def_d; + ltp_def_from_ecef_d(<p_def_d, &ref_coor_d); + + struct EcefCoor_i ref_coor_i = { rint(CM_OF_M(ref_coor_d.x)), + rint(CM_OF_M(ref_coor_d.y)), + rint(CM_OF_M(ref_coor_d.z))}; + printf("ecef0 : (%d,%d,%d)\n", ref_coor_i.x, ref_coor_i.y, ref_coor_i.z); + struct LtpDef_i ltp_def_i; + ltp_def_from_ecef_i(<p_def_i, &ref_coor_i); + + +#define STEP_NED 100.0 +#define RANGE_NED 10000. + double sum_err = 0; + struct FloatVect3 max_err; + FLOAT_VECT3_ZERO(max_err); + struct FloatVect3 offset; + for (offset.x=-RANGE_NED; offset.x<=RANGE_NED; offset.x+=STEP_NED) { + for (offset.y=-RANGE_NED; offset.y<=RANGE_NED; offset.y+=STEP_NED) { + for (offset.z=-RANGE_NED; offset.z<=RANGE_NED; offset.z+=STEP_NED) { + struct NedCoor_d my_ned_point_d; + VECT3_COPY(my_ned_point_d, offset); + struct EcefCoor_d my_ecef_point_d; + ecef_of_ned_point_d(&my_ecef_point_d, <p_def_d, &my_ned_point_d); +#if DEBUG + printf("ned to ecef double : (%.02f,%.02f,%.02f) -> (%.02f,%.02f,%.02f)\n", + my_enu_point_f.x, my_enu_point_f.y, my_enu_point_f.z, + my_ecef_point_f.x, my_ecef_point_f.y, my_ecef_point_f.z); +#endif + + struct NedCoor_i my_ned_point_i; + POSITIONS_BFP_OF_REAL(my_ned_point_i, my_ned_point_d); + struct EcefCoor_i my_ecef_point_i; + ecef_of_ned_pos_i(&my_ecef_point_i, <p_def_i, &my_ned_point_i); + +#if DEBUG + //printf("def->ecef (%d,%d,%d)\n", ltp_def_i.ecef.x, ltp_def_i.ecef.y, ltp_def_i.ecef.z); + printf("ned to ecef int : (%.2f,%.02f,%.02f) -> (%.02f,%.02f,%.02f)\n\n", + POS_BFP_OF_REAL(my_ned_point_i.x), + POS_BFP_OF_REAL(my_ned_point_i.y), + POS_BFP_OF_REAL(my_ned_point_i.z), + M_OF_CM((double)my_ecef_point_i.x), + M_OF_CM((double)my_ecef_point_i.y), + M_OF_CM((double)my_ecef_point_i.z)); +#endif + + float ex = my_ecef_point_d.x - M_OF_CM((double)my_ecef_point_i.x); + if (fabs(ex) > max_err.x) max_err.x = fabs(ex); + float ey = my_ecef_point_d.y - M_OF_CM((double)my_ecef_point_i.y); + if (fabs(ey) > max_err.y) max_err.y = fabs(ey); + float ez = my_ecef_point_d.z - M_OF_CM((double)my_ecef_point_i.z); + if (fabs(ez) > max_err.z) max_err.z = fabs(ez); + sum_err += ex*ex + ey*ey + ez*ez; + } + } + } + + double nb_samples = (2*RANGE_NED / STEP_NED + 1) * (2*RANGE_NED / STEP_NED + 1) * + (2*RANGE_NED / STEP_NED + 1); + + + printf("ecef_of_ned_pos int/float comparison:\n"); + printf("error max (%f,%f,%f) m\n", max_err.x, max_err.y, max_err.z ); + printf("error avg (%f ) m \n", sqrt(sum_err) / nb_samples ); + printf("\n"); + +} + static void test_enu_of_ecef_int(void) { @@ -126,15 +205,16 @@ static void test_enu_of_ecef_int(void) { ltp_def_from_ecef_f(<p_def_f, &ref_coor_f); struct EcefCoor_i ref_coor_i = { rint(CM_OF_M(ref_coor_f.x)), - rint(CM_OF_M(ref_coor_f.y)), - rint(CM_OF_M(ref_coor_f.z))}; + rint(CM_OF_M(ref_coor_f.y)), + rint(CM_OF_M(ref_coor_f.z))}; printf("ecef0 : (%d,%d,%d)\n", ref_coor_i.x, ref_coor_i.y, ref_coor_i.z); struct LtpDef_i ltp_def_i; ltp_def_from_ecef_i(<p_def_i, &ref_coor_i); - printf("lla0 : (%f deg, %f deg, %f m) (%f,%f,%f)\n", DegOfRad(ltp_def_f.lla.lat), DegOfRad(ltp_def_f.lla.lon), ltp_def_f.lla.alt, - DegOfRad(RAD_OF_EM7RAD((double)ltp_def_i.lla.lat)), - DegOfRad(RAD_OF_EM7RAD((double)ltp_def_i.lla.lon)), - M_OF_MM((double)ltp_def_i.lla.alt)); + printf("lla0 : float (%f deg, %f deg, %f m) , int (%f, %f, %f)\n", + DegOfRad(ltp_def_f.lla.lat), DegOfRad(ltp_def_f.lla.lon), ltp_def_f.lla.alt, + DegOfRad(RAD_OF_EM7RAD((double)ltp_def_i.lla.lat)), + DegOfRad(RAD_OF_EM7RAD((double)ltp_def_i.lla.lon)), + M_OF_MM((double)ltp_def_i.lla.alt)); #define STEP 1000. #define RANGE 100000. @@ -145,40 +225,40 @@ static void test_enu_of_ecef_int(void) { for (offset.x=-RANGE; offset.x<=RANGE; offset.x+=STEP) { for (offset.y=-RANGE; offset.y<=RANGE; offset.y+=STEP) { for (offset.z=-RANGE; offset.z<=RANGE; offset.z+=STEP) { - struct EcefCoor_f my_ecef_point_f = ref_coor_f; - VECT3_ADD(my_ecef_point_f, offset); - struct EnuCoor_f my_enu_point_f; - enu_of_ecef_point_f(&my_enu_point_f, <p_def_f, &my_ecef_point_f); + struct EcefCoor_f my_ecef_point_f = ref_coor_f; + VECT3_ADD(my_ecef_point_f, offset); + struct EnuCoor_f my_enu_point_f; + enu_of_ecef_point_f(&my_enu_point_f, <p_def_f, &my_ecef_point_f); #if DEBUG - printf("ecef to enu float : (%.02f,%.02f,%.02f) -> (%.02f,%.02f,%.02f)\n", - my_ecef_point_f.x, my_ecef_point_f.y, my_ecef_point_f.z, - my_enu_point_f.x, my_enu_point_f.y, my_enu_point_f.z ); + printf("ecef to enu float : (%.02f,%.02f,%.02f) -> (%.02f,%.02f,%.02f)\n", + my_ecef_point_f.x, my_ecef_point_f.y, my_ecef_point_f.z, + my_enu_point_f.x, my_enu_point_f.y, my_enu_point_f.z ); #endif - struct EcefCoor_i my_ecef_point_i = { rint(CM_OF_M(my_ecef_point_f.x)), - rint(CM_OF_M(my_ecef_point_f.y)), - rint(CM_OF_M(my_ecef_point_f.z))};; - struct EnuCoor_i my_enu_point_i; - enu_of_ecef_point_i(&my_enu_point_i, <p_def_i, &my_ecef_point_i); + struct EcefCoor_i my_ecef_point_i = { rint(CM_OF_M(my_ecef_point_f.x)), + rint(CM_OF_M(my_ecef_point_f.y)), + rint(CM_OF_M(my_ecef_point_f.z))};; + struct EnuCoor_i my_enu_point_i; + enu_of_ecef_point_i(&my_enu_point_i, <p_def_i, &my_ecef_point_i); #if DEBUG - // printf("def->ecef (%d,%d,%d)\n", ltp_def_i.ecef.x, ltp_def_i.ecef.y, ltp_def_i.ecef.z); - printf("ecef to enu int : (%.2f,%.02f,%.02f) -> (%.02f,%.02f,%.02f)\n\n", - M_OF_CM((double)my_ecef_point_i.x), - M_OF_CM((double)my_ecef_point_i.y), - M_OF_CM((double)my_ecef_point_i.z), - M_OF_CM((double)my_enu_point_i.x), - M_OF_CM((double)my_enu_point_i.y), - M_OF_CM((double)my_enu_point_i.z)); + //printf("def->ecef (%d,%d,%d)\n", ltp_def_i.ecef.x, ltp_def_i.ecef.y, ltp_def_i.ecef.z); + printf("ecef to enu int : (%.2f,%.02f,%.02f) -> (%.02f,%.02f,%.02f)\n\n", + M_OF_CM((double)my_ecef_point_i.x), + M_OF_CM((double)my_ecef_point_i.y), + M_OF_CM((double)my_ecef_point_i.z), + M_OF_CM((double)my_enu_point_i.x), + M_OF_CM((double)my_enu_point_i.y), + M_OF_CM((double)my_enu_point_i.z)); #endif - float ex = my_enu_point_f.x - M_OF_CM((double)my_enu_point_i.x); - if (fabs(ex) > max_err.x) max_err.x = fabs(ex); - float ey = my_enu_point_f.y - M_OF_CM((double)my_enu_point_i.y); - if (fabs(ey) > max_err.y) max_err.y = fabs(ey); - float ez = my_enu_point_f.z - M_OF_CM((double)my_enu_point_i.z); - if (fabs(ez) > max_err.z) max_err.z = fabs(ez); - sum_err += ex*ex + ey*ey + ez*ez; + float ex = my_enu_point_f.x - M_OF_CM((double)my_enu_point_i.x); + if (fabs(ex) > max_err.x) max_err.x = fabs(ex); + float ey = my_enu_point_f.y - M_OF_CM((double)my_enu_point_i.y); + if (fabs(ey) > max_err.y) max_err.y = fabs(ey); + float ez = my_enu_point_f.z - M_OF_CM((double)my_enu_point_i.z); + if (fabs(ez) > max_err.z) max_err.z = fabs(ez); + sum_err += ex*ex + ey*ey + ez*ez; } } } @@ -191,7 +271,6 @@ static void test_enu_of_ecef_int(void) { printf("error avg (%f ) m \n", sqrt(sum_err) / nb_samples ); printf("\n"); - } @@ -200,7 +279,8 @@ static void test_enu_of_ecef_int(void) { void test_ned_to_ecef_to_ned( void ) { -#if 0 + printf("\n--- NED -> ECEF -> NED in double ---\n"); + struct EcefCoor_d ref_coor = { 4624497.0 , 116475.0, 4376563.0}; printf("ecef0 : (%.02f,%.02f,%.02f)\n", ref_coor.x, ref_coor.y, ref_coor.z); @@ -211,18 +291,16 @@ void test_ned_to_ecef_to_ned( void ) { struct NedCoor_d ned_p1; ned_of_ecef_point_d(&ned_p1, <p_def, &ecef_p1); printf("ecef to ned : (%f,%f,%f) -> (%f,%f,%f)\n", - ecef_p1.x, ecef_p1.y, ecef_p1.z, - ned_p1.x, ned_p1.y, ned_p1.z ); + ecef_p1.x, ecef_p1.y, ecef_p1.z, + ned_p1.x, ned_p1.y, ned_p1.z ); struct EcefCoor_d ecef_p2; ecef_of_ned_point_d(&ecef_p2, <p_def, &ned_p1); printf("ned to ecef : (%f,%f,%f) -> (%f,%f,%f)\n", - ned_p1.x, ned_p1.y, ned_p1.z, - ecef_p2.x, ecef_p2.y, ecef_p2.z); + ned_p1.x, ned_p1.y, ned_p1.z, + ecef_p2.x, ecef_p2.y, ecef_p2.z); printf("\n"); -#endif - } @@ -233,6 +311,8 @@ void test_ned_to_ecef_to_ned( void ) { void test_enu_to_ecef_to_enu( void ) { + printf("\n--- ENU -> ECEF -> ENU in float ---\n"); + struct EcefCoor_f ref_coor = { 4624497.0 , 116475.0, 4376563.0}; printf("ecef0 : (%.02f,%.02f,%.02f)\n", ref_coor.x, ref_coor.y, ref_coor.z); @@ -243,19 +323,16 @@ void test_enu_to_ecef_to_enu( void ) { struct EnuCoor_f enu_p1; enu_of_ecef_point_f(&enu_p1, <p_def, &ecef_p1); printf("ecef to enu : (%f,%f,%f) -> (%f,%f,%f)\n", - ecef_p1.x, ecef_p1.y, ecef_p1.z, - enu_p1.x, enu_p1.y, enu_p1.z ); + ecef_p1.x, ecef_p1.y, ecef_p1.z, + enu_p1.x, enu_p1.y, enu_p1.z ); -#if 0 struct EcefCoor_f ecef_p2; ecef_of_enu_point_f(&ecef_p2, <p_def, &enu_p1); printf("enu to ecef : (%f,%f,%f) -> (%f,%f,%f)\n", - enu_p1.x, enu_p1.y, enu_p1.z, - ecef_p2.x, ecef_p2.y, ecef_p2.z); + enu_p1.x, enu_p1.y, enu_p1.z, + ecef_p2.x, ecef_p2.y, ecef_p2.z); printf("\n"); -#endif - } 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/extras/chdk/pictuav.lua b/sw/extras/chdk/pictuav.lua new file mode 100644 index 0000000000..07c56de78f --- /dev/null +++ b/sw/extras/chdk/pictuav.lua @@ -0,0 +1,168 @@ +--[[ +@title PictUAV +@param d Display off frame 0=never +@default d 0 +--]] + +-- Developed on IXUS 230 HS. Other cameras should work as well, but some errors or crashes are possible +-- also considering that some firmwares still have CHDK bugs. +-- +-- Other settings in the camera that you should try to manipulate to reduce the time between taking pictures: +-- CHDK menu +-- Extra Photo overrides +-- Disable overrides : Off +-- Do not override shutter speed ( I allow the camera to determine exposure time, but feel free to experiment) +-- ND filter state: Out (removes ND filter to allow more light to fall on the sensor ) +-- Override subj. dist. value: 65535. (good to keep it there) +-- Do not save RAW images, they take longer +-- +-- Camera menu +-- Use P mode, certainly not automatic +-- If you can hardset the focus, do this towards infinity +-- Play around with aperture. Smaller apertures are better for more sharpness, but not every camera allows you to. +-- (Some cameras have really bad behaviour on larger apertures that allow more light through, especially at edges). +-- Disable IS (plane vibrations mess up its function and increases the chance of blur) +-- Take Large images with Fine resolution. +-- Turn "Review" mode off. +-- Consider hard-setting ISO, but consider local weather. If set too high, shutter time goes up, which causes blur. +-- Blur can then also occur in areas where there is little light reflection from the earth. +-- +-- How to use the script: +-- Load this on the card under "CHDK/SCRIPTS" +-- Enter the CHDK menu through your "ALT" button +-- Under scripts, select the script and specify "Autostart: on" +-- +-- As the camera starts up, this also loads. with the shutter button pressed, you can interrupt the script +-- Then press the "ALT" button to disable the scripting actuator. +-- Press the shutter button to extend the lens +-- Press "ALT" again to bring up the scripting actuator. +-- Press the shutter button to reinitiate the script. +-- If you have a IXUS 230HS like me, the focus can't be set automatically. Point the camera at a distant object while the script +-- is starting. It should say "Focused", after which it's ready for use. +-- +-- Example paparazzi airframe configuration: +-- +--
+-- +-- +-- +-- +--
+-- +-- +-- +-- +-- +-- +-- +-- + +print( "PictUAV Started " ) + +function print_status (frame) + local free = get_jpg_count() + print("#" .. frame ) +end + +-- switch to autofocus mode, pre-focus, then go to manual focus mode (locking focus there). +-- this helps to reduce the delay between the signal and taking the picture. +function pre_focus() + local focused = false + local try = 1 + while not focused and try <= 5 do + print("Pre-focus attempt " .. try) + press("shoot_half") + sleep(2000) + if get_prop(18) > 0 then + print("Focused") + focused = true + set_aflock(1) + end + release("shoot_half") + sleep(500) + try = try + 1 + end + return focused +end + +-- set aperture/shooting mode to landscape +set_prop(6,3) + +ap = get_prop(6) +print ("AF(3=inf,4=MF) "..ap) + +-- Turn IS off +set_prop(145, 3) + +-- set P mode +set_capture_mode(2) +sleep(1000) + +-- Get focusing mode +p = get_prop(6) +if (p==3) then + print "Inf set." +else + -- on ixus230hs, no explicit MF. + -- so set to infinity (3) + while (p ~= 3) do + press "left" + release "left" + p = get_prop (6) + end + print "Focus set to infinity" +end + +-- on ixus230hs set focus doesn't fail, but doesn't do anything. +set_focus(100) +print "set_focus 100" +sleep(2000) +f = get_focus + +-- on ixus230hs set focus doesn't fail, but doesn't do anything. +set_focus( 65535 ) +print "set_focus 65535" +sleep( 2000) +g = get_focus + +if (f==g) then + print "set_focus inop" + -- if focusing until here didn't work, pre-focus using a different method. + pre_focus() +else + -- set_aflock(1) fails when the camera isn't knowingly focused. + print( "Setting aflock 1" ) + sleep(1000) + set_aflock( 1 ) +end + +-- measuring the pulse on CHDK isn't necessarily that accurate, they can easily vary by 40ms. +-- Since I'm using a 100ms wait here, the variance for a shoot is around 140ms. +-- If the pulse is 250ms, then I should allow for anything down to 110. +-- For Paparazzi, taking a single picture using the button may not work because the timing may be very off +-- considering the 4Hz loop (this requires a change in the cam module for paparazzi). + +print( "PictUAV loop " ) +a = -1 +repeat + a = get_usb_power(0) + -- a pulse is detected longer than ~610ms. + if (a>61) then + print( "shutting down " ) + shut_down() + sleep(1500) + break + end + -- a pulse longer than ~100ms is detected. + if (a>10) then + + frame = 1 + shoot() + + frame = frame + 1 + + end + sleep(100) +until ( false ) +print( "PictUAV ended " ) + 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/in_progress/python/attitude_viz.py b/sw/in_progress/python/attitude_viz.py index ebc5074440..325c5d3b7f 100755 --- a/sw/in_progress/python/attitude_viz.py +++ b/sw/in_progress/python/attitude_viz.py @@ -18,353 +18,362 @@ import os _NAME = 'attitude_viz' + class TelemetryQuat: - def __init__(self, message_name, index, name, integer): - self.message_name = message_name - self.index = index - self.name = name - self.qi = 1 - self.qx = 0 - self.qy = 0 - self.qz = 0 - # optional scaling for fixed point telemetry - if integer: - self.scale = 0.00003051757812 - else: - self.scale = 1.0 + def __init__(self, message_name, index, name, integer): + self.message_name = message_name + self.index = index + self.name = name + self.qi = 1 + self.qx = 0 + self.qy = 0 + self.qz = 0 + # optional scaling for fixed point telemetry + if integer: + self.scale = 0.00003051757812 + else: + self.scale = 1.0 + class TelemetryValue: - def __init__(self, message_name, index, name, offset, scale, max): - self.message_name = message_name - self.index = index - self.name = name - self.offset = offset - self.scale = scale - self.max = max - self.value = 0 + def __init__(self, message_name, index, name, offset, scale, max): + self.message_name = message_name + self.index = index + self.name = name + self.offset = offset + self.scale = scale + self.max = max + self.value = 0 + class Visualization: - def __init__(self, parent): - self.quats = [] - self.graph_values = [] - self.throttle = 0.0 - self.mode = 0.0 - self.airspeed = 0.0 - self.display_list = None - self.display_dirty = True - self.rotate_theta = parent.rotate_theta - - for message_name, index, name, bfp in VEHICLE_QUATS: - self.quats.append(TelemetryQuat(message_name, index, name, bfp)) - for message_name, index, name, offset, scale, max in BAR_VALUES: - self.graph_values.append(TelemetryValue(message_name, index, name, offset, scale, max)) - - def onmsgproc(self, agent, *larg): - data = str(larg[0]).split(' ') - for telemetry_quat in self.quats: - if (telemetry_quat.message_name == data[1]): + def __init__(self, parent): + self.quats = [] + self.graph_values = [] + self.throttle = 0.0 + self.mode = 0.0 + self.airspeed = 0.0 + self.display_list = None self.display_dirty = True - telemetry_quat.qi = float(data[telemetry_quat.index + 0]) - telemetry_quat.qx = float(data[telemetry_quat.index + 1]) - telemetry_quat.qy = float(data[telemetry_quat.index + 2]) - telemetry_quat.qz = float(data[telemetry_quat.index + 3]) + self.rotate_theta = parent.rotate_theta - for graph_value in self.graph_values: - if (graph_value.message_name == data[1]): - self.display_dirty = True - graph_value.value = (float(data[graph_value.index + 0]) + graph_value.offset) / graph_value.scale + for message_name, index, name, bfp in VEHICLE_QUATS: + self.quats.append(TelemetryQuat(message_name, index, name, bfp)) + for message_name, index, name, offset, scale, max in BAR_VALUES: + self.graph_values.append(TelemetryValue(message_name, index, name, offset, scale, max)) - def DrawCircle(self, radius): - glBegin(GL_TRIANGLE_FAN) - glVertex3f(0, 0, 0) - for angle in range (0, 361, 12): - glVertex3f( math.sin(math.radians(angle)) * radius, math.cos(math.radians(angle)) * radius, 0) - glEnd() + def onmsgproc(self, agent, *larg): + data = str(larg[0]).split(' ') + for telemetry_quat in self.quats: + if telemetry_quat.message_name == data[1]: + self.display_dirty = True + telemetry_quat.qi = float(data[telemetry_quat.index + 0]) + telemetry_quat.qx = float(data[telemetry_quat.index + 1]) + telemetry_quat.qy = float(data[telemetry_quat.index + 2]) + telemetry_quat.qz = float(data[telemetry_quat.index + 3]) - # draw quad centered at origin, z = 0 - def DrawQuad(self, width, height): - glBegin (GL_QUADS) - glVertex3f( width, height, 0) - glVertex3f( -width, height, 0) - glVertex3f( -width, -height, 0) - glVertex3f( width, -height, 0) - glEnd() + for graph_value in self.graph_values: + if graph_value.message_name == data[1]: + self.display_dirty = True + graph_value.value = (float(data[graph_value.index + 0]) + graph_value.offset) / graph_value.scale - def DrawBox(self, width, height, depth): - glPushMatrix() - glTranslate(0, 0, depth) - self.DrawQuad(width, height) - glTranslate(0, 0, -2 * depth) - self.DrawQuad(width, height) - glPopMatrix() + def DrawCircle(self, radius): + glBegin(GL_TRIANGLE_FAN) + glVertex3f(0, 0, 0) + for angle in range(0, 361, 12): + glVertex3f(math.sin(math.radians(angle)) * radius, math.cos(math.radians(angle)) * radius, 0) + glEnd() - glPushMatrix() - glRotate(90, 1, 0, 0) - glTranslate(0, 0, height) - self.DrawQuad(width, depth) - glTranslate(0, 0, -2 * height) - self.DrawQuad(width, depth) - glPopMatrix() + # draw quad centered at origin, z = 0 + def DrawQuad(self, width, height): + glBegin(GL_QUADS) + glVertex3f(width, height, 0) + glVertex3f(-width, height, 0) + glVertex3f(-width, -height, 0) + glVertex3f(width, -height, 0) + glEnd() - glPushMatrix() - glRotate(90, 0, 1, 0) - glTranslate(0, 0, width) - self.DrawQuad(depth, height) - glTranslate(0, 0, -2 * width) - self.DrawQuad(depth, height) - glPopMatrix() - - def DrawVehicle(self, name): - wingspan = 2.7 - separation = 0.7 - chord = 0.35 - thickness = 0.08 - strutcount = 3 - discradius = 0.45 - discseparation = 0.01 - - #wings - glColor3f(0.1, 0.1, 0.9) - glPushMatrix() - glTranslate(0, 0, 0.05) - self.DrawBox(wingspan, chord, thickness) - glColor3f(0.0, 0.0, 0.0) - glTranslate(-wingspan, -0.2, thickness + 0.01) - glScale(0.004, 0.004, 0.004) - for c in name: - glutStrokeCharacter(GLUT_STROKE_ROMAN, ord(c)) - glPopMatrix() - - glPushMatrix() - glTranslate(0, 0, -0.05) - glColor3f(0.6, 0.6, 0.2) - self.DrawBox(wingspan, chord, thickness) - glColor3f(0.0, 0.0, 0.0) - glTranslate(wingspan, -0.2, -0.01 - thickness) - glScale(0.004, 0.004, 0.004) - glRotate(180, 0, 1, 0) - for c in name: - glutStrokeCharacter(GLUT_STROKE_ROMAN, ord(c)) - glPopMatrix() - - if self.display_list is None: - self.display_list = glGenLists(1) - glNewList(self.display_list, GL_COMPILE) - # struts - glColor3f(0.4, 0.4, 0.4) - glPushMatrix() - glTranslate(-wingspan/2, 0, separation/2) - glRotate(90, 0, 1, 0) - for x in range (0, strutcount-1): - self.DrawBox(separation/2, chord - .01, thickness) - glTranslate(0, 0, wingspan) - glTranslate(separation, 0, -5*wingspan/2) - for x in range (0, strutcount-1): - self.DrawBox(separation/2, chord - .01, thickness) - glTranslate(0, 0, 2*wingspan) - glPopMatrix() - - #rotors - glColor3f(0.9, 0.1, 0.1) - glPushMatrix() - glRotate(90, 1, 0, 0) - glTranslate(-wingspan/2, separation, -(chord + .01)) - for x in range (0, strutcount): - if (x != strutcount/2): - self.DrawCircle(discradius) - glTranslate(2 * wingspan/(strutcount + 1), 0, 0) - glPopMatrix() - - glPushMatrix() - glRotate(90, 1, 0, 0) - glTranslate(-wingspan, -separation, -(chord + .01)) - for x in range (0, strutcount): - if (x != strutcount/2): - self.DrawCircle(discradius) - glTranslate(2 * wingspan/(strutcount - 1), 0, 0) - glPopMatrix() - glEndList() - - glCallList(self.display_list) - - def DrawBar(self, name, value): - bar_height = 0.12 - bar_length = 3 - glPushMatrix() - glColor3f(0, 0, 0) - glTranslate(-bar_length, -0.09, 0.02) - glScale(0.0015, 0.0015, 0.0015) - for c in name: - glutStrokeCharacter(GLUT_STROKE_ROMAN, ord(c)) - glPopMatrix() - glColor3f(0.92, 0.92, 0.92) - glPushMatrix() - glTranslate(0, 0, 0) - self.DrawQuad(bar_length, bar_height) - glPopMatrix() - glPushMatrix() - glTranslate(bar_length * value - bar_length, 0, 0.01) - glColor3f(0.6, 0.6, 0.6) - self.DrawQuad(bar_length * value, bar_height) - glPopMatrix() - - def Draw(self): - glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) - - glPushMatrix() - - height = 5 - - glDisable(GL_LIGHTING) - glPushMatrix() - for graph_value in self.graph_values: - self.DrawBar(graph_value.name % (graph_value.value), graph_value.value / graph_value.max) - glTranslate(0, 0.35, 0) - glPopMatrix() - glEnable(GL_LIGHTING) - - glTranslate(0, -height + (height / len(self.quats) + 1), 0) - for telemetry_quat in self.quats: - glPushMatrix() - try: - scaled_quat = [telemetry_quat.qi * telemetry_quat.scale, telemetry_quat.qx * telemetry_quat.scale, telemetry_quat.qy * telemetry_quat.scale, telemetry_quat.qz * telemetry_quat.scale] - glRotate(360 * math.acos(scaled_quat[0] ) / math.pi, scaled_quat[2], -scaled_quat[3], -scaled_quat[1]) - glRotate(self.rotate_theta, 1, 0, 0) - - self.DrawVehicle(telemetry_quat.name) - except Exception: - raise Exception - finally: + def DrawBox(self, width, height, depth): + glPushMatrix() + glTranslate(0, 0, depth) + self.DrawQuad(width, height) + glTranslate(0, 0, -2 * depth) + self.DrawQuad(width, height) glPopMatrix() - glTranslate(0, 2 * height / (len(self.quats)), 0) - glPopMatrix() + + glPushMatrix() + glRotate(90, 1, 0, 0) + glTranslate(0, 0, height) + self.DrawQuad(width, depth) + glTranslate(0, 0, -2 * height) + self.DrawQuad(width, depth) + glPopMatrix() + + glPushMatrix() + glRotate(90, 0, 1, 0) + glTranslate(0, 0, width) + self.DrawQuad(depth, height) + glTranslate(0, 0, -2 * width) + self.DrawQuad(depth, height) + glPopMatrix() + + def DrawVehicle(self, name): + wingspan = 2.7 + separation = 0.7 + chord = 0.35 + thickness = 0.08 + strutcount = 3 + discradius = 0.45 + discseparation = 0.01 + + #wings + glColor3f(0.1, 0.1, 0.9) + glPushMatrix() + glTranslate(0, 0, 0.05) + self.DrawBox(wingspan, chord, thickness) + glColor3f(0.0, 0.0, 0.0) + glTranslate(-wingspan, -0.2, thickness + 0.01) + glScale(0.004, 0.004, 0.004) + for c in name: + glutStrokeCharacter(GLUT_STROKE_ROMAN, ord(c)) + glPopMatrix() + + glPushMatrix() + glTranslate(0, 0, -0.05) + glColor3f(0.6, 0.6, 0.2) + self.DrawBox(wingspan, chord, thickness) + glColor3f(0.0, 0.0, 0.0) + glTranslate(wingspan, -0.2, -0.01 - thickness) + glScale(0.004, 0.004, 0.004) + glRotate(180, 0, 1, 0) + for c in name: + glutStrokeCharacter(GLUT_STROKE_ROMAN, ord(c)) + glPopMatrix() + + if self.display_list is None: + self.display_list = glGenLists(1) + glNewList(self.display_list, GL_COMPILE) + # struts + glColor3f(0.4, 0.4, 0.4) + glPushMatrix() + glTranslate(-wingspan / 2, 0, separation / 2) + glRotate(90, 0, 1, 0) + for x in range(0, strutcount - 1): + self.DrawBox(separation / 2, chord - .01, thickness) + glTranslate(0, 0, wingspan) + glTranslate(separation, 0, -5 * wingspan / 2) + for x in range(0, strutcount - 1): + self.DrawBox(separation / 2, chord - .01, thickness) + glTranslate(0, 0, 2 * wingspan) + glPopMatrix() + + #rotors + glColor3f(0.9, 0.1, 0.1) + glPushMatrix() + glRotate(90, 1, 0, 0) + glTranslate(-wingspan / 2, separation, -(chord + .01)) + for x in range(0, strutcount): + if (x != strutcount / 2): + self.DrawCircle(discradius) + glTranslate(2 * wingspan / (strutcount + 1), 0, 0) + glPopMatrix() + + glPushMatrix() + glRotate(90, 1, 0, 0) + glTranslate(-wingspan, -separation, -(chord + .01)) + for x in range(0, strutcount): + if (x != strutcount / 2): + self.DrawCircle(discradius) + glTranslate(2 * wingspan / (strutcount - 1), 0, 0) + glPopMatrix() + glEndList() + + glCallList(self.display_list) + + def DrawBar(self, name, value): + bar_height = 0.12 + bar_length = 3 + glPushMatrix() + glColor3f(0, 0, 0) + glTranslate(-bar_length, -0.09, 0.02) + glScale(0.0015, 0.0015, 0.0015) + for c in name: + glutStrokeCharacter(GLUT_STROKE_ROMAN, ord(c)) + glPopMatrix() + glColor3f(0.92, 0.92, 0.92) + glPushMatrix() + glTranslate(0, 0, 0) + self.DrawQuad(bar_length, bar_height) + glPopMatrix() + glPushMatrix() + glTranslate(bar_length * value - bar_length, 0, 0.01) + glColor3f(0.6, 0.6, 0.6) + self.DrawQuad(bar_length * value, bar_height) + glPopMatrix() + + def Draw(self): + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) + + glPushMatrix() + + height = 5 + + glDisable(GL_LIGHTING) + glPushMatrix() + for graph_value in self.graph_values: + self.DrawBar(graph_value.name % (graph_value.value), graph_value.value / graph_value.max) + glTranslate(0, 0.35, 0) + glPopMatrix() + glEnable(GL_LIGHTING) + + glTranslate(0, -height + (height / len(self.quats) + 1), 0) + for telemetry_quat in self.quats: + glPushMatrix() + try: + scaled_quat = [telemetry_quat.qi * telemetry_quat.scale, telemetry_quat.qx * telemetry_quat.scale, + telemetry_quat.qy * telemetry_quat.scale, telemetry_quat.qz * telemetry_quat.scale] + glRotate(360 * math.acos(scaled_quat[0]) / math.pi, scaled_quat[2], -scaled_quat[3], -scaled_quat[1]) + glRotate(self.rotate_theta, 1, 0, 0) + + self.DrawVehicle(telemetry_quat.name) + except Exception: + raise Exception + finally: + glPopMatrix() + glTranslate(0, 2 * height / (len(self.quats)), 0) + glPopMatrix() + class Visualizer: - def __init__(self, rotate_theta): - self.rotate_theta = rotate_theta - self.visualization = Visualization(self) + def __init__(self, rotate_theta): + self.rotate_theta = rotate_theta + self.visualization = Visualization(self) - # listen to Ivy - logging.getLogger('Ivy').setLevel(logging.WARN) - IvyInit(_NAME, - "", - 0, - lambda x, y: y, - lambda x, z: z - ) + # listen to Ivy + logging.getLogger('Ivy').setLevel(logging.WARN) + IvyInit(_NAME, + "", + 0, + lambda x, y: y, + lambda x, z: z) - if os.getenv('IVY_BUS') != None: - IvyStart(os.getenv('IVY_BUS')) - else: - if platform.system() == 'Darwin': - IvyStart("224.255.255.255:2010") - else: - IvyStart() + if os.getenv('IVY_BUS') is not None: + IvyStart(os.getenv('IVY_BUS')) + else: + if platform.system() == 'Darwin': + IvyStart("224.255.255.255:2010") + else: + IvyStart() - # list of all message names - messages = [] + # list of all message names + messages = [] - # append all message names - for vehicle_quat in VEHICLE_QUATS: - messages.append(vehicle_quat[0]) - for bar_value in BAR_VALUES: - messages.append(bar_value[0]) + # append all message names + for vehicle_quat in VEHICLE_QUATS: + messages.append(vehicle_quat[0]) + for bar_value in BAR_VALUES: + messages.append(bar_value[0]) - # bind to set of messages (ie, only bind each message once) - for message_name in set(messages): - bind_string = "(^.*" + message_name + ".*$)" - IvyBindMsg(self.visualization.onmsgproc, bind_string) + # bind to set of messages (ie, only bind each message once) + for message_name in set(messages): + bind_string = "(^.*" + message_name + ".*$)" + IvyBindMsg(self.visualization.onmsgproc, bind_string) - def Draw(self): - if self.visualization.display_dirty: - self.visualization.Draw() - self.visualization.display_dirty = False + def Draw(self): + if self.visualization.display_dirty: + self.visualization.Draw() + self.visualization.display_dirty = False + + def OnClose(self): + IvyStop() - def OnClose(self): - IvyStop() SCREEN_SIZE = (800, 800) + def resize(width, height): - glViewport(0, 0, width, height) - glMatrixMode(GL_PROJECTION) - glLoadIdentity() - gluPerspective(60.0, float(width/height), .1, 100.) - glMatrixMode(GL_MODELVIEW) - glLoadIdentity() + glViewport(0, 0, width, height) + glMatrixMode(GL_PROJECTION) + glLoadIdentity() + gluPerspective(60.0, float(width / height), .1, 100.) + glMatrixMode(GL_MODELVIEW) + glLoadIdentity() + def init(): - glutInit() - glEnable(GL_LINE_SMOOTH) - glEnable(GL_DEPTH_TEST) - glEnable(GL_LIGHTING) - glEnable(GL_LIGHT0) - glEnable(GL_BLEND) - glShadeModel (GL_SMOOTH) - glClearColor(1.0, 1.0, 1.0, 1.0) - glClearDepth(1.0) + glutInit() + glEnable(GL_LINE_SMOOTH) + glEnable(GL_DEPTH_TEST) + glEnable(GL_LIGHTING) + glEnable(GL_LIGHT0) + glEnable(GL_BLEND) + glShadeModel(GL_SMOOTH) + glClearColor(1.0, 1.0, 1.0, 1.0) + glClearDepth(1.0) - glPointSize(3.0) + glPointSize(3.0) - glMatrixMode(GL_PROJECTION) - glLoadIdentity() - gluPerspective(7.0, 1.0, 95.0, 105.0) + glMatrixMode(GL_PROJECTION) + glLoadIdentity() + gluPerspective(7.0, 1.0, 95.0, 105.0) - glMatrixMode(GL_MODELVIEW) + glMatrixMode(GL_MODELVIEW) - glLight(GL_LIGHT0, GL_POSITION, [5, 30, -20]) - glLight(GL_LIGHT0, GL_AMBIENT, [0.5, 0.5, 0.5]) - glLight(GL_LIGHT0, GL_SPECULAR, [0.0, 0.0, 0.0]) - glLight(GL_LIGHT0, GL_DIFFUSE, [0.8, 0.8, 0.8]) - glEnable(GL_COLOR_MATERIAL) - glColorMaterial(GL_FRONT, GL_AMBIENT_AND_DIFFUSE) + glLight(GL_LIGHT0, GL_POSITION, [5, 30, -20]) + glLight(GL_LIGHT0, GL_AMBIENT, [0.5, 0.5, 0.5]) + glLight(GL_LIGHT0, GL_SPECULAR, [0.0, 0.0, 0.0]) + glLight(GL_LIGHT0, GL_DIFFUSE, [0.8, 0.8, 0.8]) + glEnable(GL_COLOR_MATERIAL) + glColorMaterial(GL_FRONT, GL_AMBIENT_AND_DIFFUSE) + + glMatrixMode(GL_MODELVIEW) + glLoadIdentity() + gluLookAt(0.0, 0.0, 100.0, + 0.0, 0.0, 0.0, + 0.0, 1.0, 0.0) - glMatrixMode(GL_MODELVIEW) - glLoadIdentity() - gluLookAt(0.0, 0.0, 100.0, - 0.0, 0.0, 0.0, - 0.0, 1.0, 0.0) def run(): - global VEHICLE_QUATS, BAR_VALUES - VEHICLE_QUATS = [ ["AHRS_REF_QUAT", 6, "Estimate", True], ["AHRS_REF_QUAT", 2, "Reference", True]] - BAR_VALUES = [ ["ROTORCRAFT_RADIO_CONTROL", 5, "Throttle (%%) %i", 0, 100, 100] ] - window_title = "Attitude_Viz" - rotate_theta = -90 - try: - opts, args = getopt.getopt(sys.argv[1:], "t:r:", ["title", "rotate_theta"]) - for o, a in opts: - if o in ("-t", "--title"): - window_title = a - if o in ("-r", "--rotate_theta"): - rotate_theta = int(a) - except getopt.error as msg: - print(msg) - print("""usage: + global VEHICLE_QUATS, BAR_VALUES + VEHICLE_QUATS = [["AHRS_REF_QUAT", 6, "Estimate", True], ["AHRS_REF_QUAT", 2, "Reference", True]] + BAR_VALUES = [["ROTORCRAFT_RADIO_CONTROL", 5, "Throttle (%%) %i", 0, 100, 100]] + window_title = "Attitude_Viz" + rotate_theta = -90 + try: + opts, args = getopt.getopt(sys.argv[1:], "t:r:", ["title", "rotate_theta"]) + for o, a in opts: + if o in ("-t", "--title"): + window_title = a + if o in ("-r", "--rotate_theta"): + rotate_theta = int(a) + except getopt.error as msg: + print(msg) + print("""usage: -t, --title set window title -r, --rotate_theta rotate the quaternion by n degrees over the pitch axis (default: -90) """) - pygame.init() - screen = pygame.display.set_mode(SCREEN_SIZE, pygame.OPENGL|pygame.DOUBLEBUF) - #resize(*SCREEN_SIZE) - init() - visualizer = Visualizer(rotate_theta) + pygame.init() + screen = pygame.display.set_mode(SCREEN_SIZE, pygame.OPENGL | pygame.DOUBLEBUF) + #resize(*SCREEN_SIZE) + init() + visualizer = Visualizer(rotate_theta) + + try: + while True: + for event in pygame.event.get(): + if event.type == pygame.QUIT: + visualizer.OnClose() + return + if event.type == pygame.KEYUP and event.key == pygame.K_ESCAPE: + visualizer.OnClose() + return + visualizer.Draw() + pygame.display.flip() + time.sleep(.02) + except KeyboardInterrupt: + visualizer.OnClose() + return - try: - while True: - for event in pygame.event.get(): - if event.type == pygame.QUIT: - visualizer.OnClose() - return - if event.type == pygame.KEYUP and event.key == pygame.K_ESCAPE: - visualizer.OnClose() - return - visualizer.Draw() - pygame.display.flip() - time.sleep(.02) - except KeyboardInterrupt: - visualizer.OnClose() - return if __name__ == "__main__": - run() + run() 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 @@ + + + True + False + False + _Get Help + True + + + 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..082545f995 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 @@ -29,6 +30,7 @@ from scipy import optimize import calibration_utils + def main(): usage = "usage: %prog [options] log_filename.data" + "\n" + "Run %prog --help to list the options." parser = OptionParser(usage) @@ -42,6 +44,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() @@ -54,7 +59,7 @@ def main(): print(args[0] + " not found") sys.exit(1) ac_ids = calibration_utils.get_ids_in_log(filename) - if options.ac_id == None: + if options.ac_id is None: if len(ac_ids) == 1: options.ac_id = ac_ids[0] else: @@ -65,13 +70,13 @@ def main(): if options.sensor == "ACCEL": sensor_ref = 9.81 sensor_res = 10 - noise_window = 20; - noise_threshold = 40; + noise_window = 20 + noise_threshold = 40 elif options.sensor == "MAG": sensor_ref = 1. sensor_res = 11 - noise_window = 10; - noise_threshold = 1000; + noise_window = 10 + noise_threshold = 1000 if not filename.endswith(".data"): parser.error("Please specify a *.data log file") @@ -84,14 +89,23 @@ def main(): print("Error: found zero IMU_"+options.sensor+"_RAW measurements for aircraft with id "+options.ac_id+" in log file!") sys.exit(1) if options.verbose: - print("found "+str(len(measurements))+" records") + 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/calibrate_gyro.py b/sw/tools/calibration/calibrate_gyro.py index 7f2970e0fd..77aac9f076 100755 --- a/sw/tools/calibration/calibrate_gyro.py +++ b/sw/tools/calibration/calibrate_gyro.py @@ -87,7 +87,7 @@ def main(): if options.verbose: print("reading file "+filename+" for aircraft "+str(options.ac_id)+" and turntable "+str(options.tt_id)) - samples = calibration_utils.read_turntable_log(options.ac_id, options.tt_id, filename, 1, 7) + samples = calibration_utils.read_turntable_log(options.ac_id, options.tt_id, filename, 1, 7) if len(samples) == 0: print("Error: found zero matching messages in log file!") @@ -106,13 +106,13 @@ def main(): parser.error("Specify a valid axis!") #Linear regression using stats.linregress - t = samples[:, 0] + t = samples[:, 0] xn = samples[:, axis_idx] - (a_s, b_s, r, tt, stderr)=stats.linregress(t, xn) + (a_s, b_s, r, tt, stderr) = stats.linregress(t, xn) print('Linear regression using stats.linregress') print(('regression: a=%.2f b=%.2f, std error= %.3f' % (a_s, b_s, stderr))) - print(('' % (b_s))); - print(('' % (pow(2, 12)/a_s))); + print(('' % (b_s))) + print(('' % (pow(2, 12)/a_s))) # # overlay fited value @@ -125,7 +125,7 @@ def main(): plot(samples[:, 1]) plot(samples[:, 2]) plot(samples[:, 3]) - legend(['p', 'q', 'r']); + legend(['p', 'q', 'r']) subplot(3, 1, 2) plot(samples[:, 0]) @@ -134,7 +134,7 @@ def main(): plot(samples[:, 0], samples[:, axis_idx], 'b.') plot(ovl_omega, ovl_adc, 'r') - show(); + show() if __name__ == "__main__": diff --git a/sw/tools/calibration/calibrate_mag_current.py b/sw/tools/calibration/calibrate_mag_current.py index d93f6cfc64..6779f0f9a1 100755 --- a/sw/tools/calibration/calibrate_mag_current.py +++ b/sw/tools/calibration/calibrate_mag_current.py @@ -49,7 +49,7 @@ def main(): print(args[0] + " not found") sys.exit(1) ac_ids = calibration_utils.get_ids_in_log(filename) - if options.ac_id == None: + if options.ac_id is None: if len(ac_ids) == 1: options.ac_id = ac_ids[0] else: @@ -57,10 +57,6 @@ def main(): if options.verbose: print("Using aircraft id "+options.ac_id) - sensor_ref = 1. - sensor_res = 11 - noise_window = 10; - noise_threshold = 1000; if not filename.endswith(".data"): parser.error("Please specify a *.data log file") @@ -73,7 +69,7 @@ def main(): print("Error: found zero IMU_MAG_CURRENT_CALIBRATION measurements for aircraft with id "+options.ac_id+" in log file!") sys.exit(1) if options.verbose: - print("found "+str(len(measurements))+" records") + print("found "+str(len(measurements))+" records") coefficient = calibration_utils.estimate_mag_current_relation(measurements) diff --git a/sw/tools/calibration/calibration_utils.py b/sw/tools/calibration/calibration_utils.py index c079fd8bb9..4fc13aab6c 100644 --- a/sw/tools/calibration/calibration_utils.py +++ b/sw/tools/calibration/calibration_utils.py @@ -38,7 +38,7 @@ def get_ids_in_log(filename): line = f.readline().strip() if line == '': break - m=re.match(pattern, line) + m = re.match(pattern, line) if m: id = m.group(1) if not id in ids: @@ -56,7 +56,7 @@ def read_log(ac_id, filename, sensor): line = f.readline().strip() if line == '': break - m=re.match(pattern, line) + m = re.match(pattern, line) if m: list_meas.append([float(m.group(2)), float(m.group(3)), float(m.group(4))]) return scipy.array(list_meas) @@ -72,7 +72,7 @@ def read_log_mag_current(ac_id, filename): line = f.readline().strip() if line == '': break - m=re.match(pattern, line) + m = re.match(pattern, line) if m: list_meas.append([float(m.group(2)), float(m.group(3)), float(m.group(4)), float(m.group(5))]) return scipy.array(list_meas) @@ -106,8 +106,8 @@ def get_min_max_guess(meas, scale): # scale the set of measurements # def scale_measurements(meas, p): - l_comp = []; - l_norm = []; + l_comp = [] + l_norm = [] for m in meas[:,]: sm = (m - p[0:3])*p[3:6] l_comp.append(sm) @@ -120,7 +120,7 @@ def scale_measurements(meas, p): def estimate_mag_current_relation(meas): coefficient = [] for i in range(0,3): - gradient, intercept, r_value, p_value, std_err = stats.linregress(meas[:,3],meas[:,i]) + gradient, intercept, r_value, p_value, std_err = stats.linregress(meas[:,3], meas[:,i]) coefficient.append(gradient) return coefficient @@ -154,26 +154,26 @@ def plot_results(block, measurements, flt_idx, flt_meas, cp0, np0, cp1, np1, sen title('Raw sensors') subplot(3, 2, 3) - plot(cp0[:, 0]); - plot(cp0[:, 1]); - plot(cp0[:, 2]); - plot(-sensor_ref*scipy.ones(len(flt_meas))); - plot(sensor_ref*scipy.ones(len(flt_meas))); + plot(cp0[:, 0]) + plot(cp0[:, 1]) + plot(cp0[:, 2]) + plot(-sensor_ref*scipy.ones(len(flt_meas))) + plot(sensor_ref*scipy.ones(len(flt_meas))) subplot(3, 2, 4) - plot(np0); - plot(sensor_ref*scipy.ones(len(flt_meas))); + plot(np0) + plot(sensor_ref*scipy.ones(len(flt_meas))) subplot(3, 2, 5) - plot(cp1[:, 0]); - plot(cp1[:, 1]); - plot(cp1[:, 2]); - plot(-sensor_ref*scipy.ones(len(flt_meas))); - plot(sensor_ref*scipy.ones(len(flt_meas))); + plot(cp1[:, 0]) + plot(cp1[:, 1]) + plot(cp1[:, 2]) + plot(-sensor_ref*scipy.ones(len(flt_meas))) + plot(sensor_ref*scipy.ones(len(flt_meas))) subplot(3, 2, 6) - plot(np1); - plot(sensor_ref*scipy.ones(len(flt_meas))); + plot(np1) + plot(sensor_ref*scipy.ones(len(flt_meas))) # if we want to have another plot we only draw the figure (non-blocking) # also in matplotlib before 1.0.0 there is only one call to show possible @@ -187,20 +187,19 @@ def plot_results(block, measurements, flt_idx, flt_meas, cp0, np0, cp1, np1, sen # def plot_mag_3d(measured, calibrated, p): # set up points for sphere and ellipsoid wireframes - u=r_[0:2*pi:20j] - v=r_[0:pi:20j] - wx=outer(cos(u),sin(v)) - wy=outer(sin(u),sin(v)) - wz=outer(ones(size(u)),cos(v)) - ex=p[0]*ones(size(u)) + outer(cos(u),sin(v))/p[3] - ey=p[1]*ones(size(u)) + outer(sin(u),sin(v))/p[4] - ez=p[2]*ones(size(u)) + outer(ones(size(u)),cos(v))/p[5] + u = r_[0:2 * pi:20j] + v = r_[0:pi:20j] + wx = outer(cos(u), sin(v)) + wy = outer(sin(u), sin(v)) + wz = outer(ones(size(u)), cos(v)) + ex = p[0] * ones(size(u)) + outer(cos(u), sin(v)) / p[3] + ey = p[1] * ones(size(u)) + outer(sin(u), sin(v)) / p[4] + ez = p[2] * ones(size(u)) + outer(ones(size(u)), cos(v)) / p[5] # measurements mx = measured[:, 0] my = measured[:, 1] mz = measured[:, 2] - m_max = amax(abs(measured)) # calibrated values cx = calibrated[:, 0] @@ -224,17 +223,23 @@ 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) @@ -267,10 +272,10 @@ def read_turntable_log(ac_id, tt_id, filename, _min, _max): line = f.readline().strip() if line == '': break - m=re.match(pattern_t, line) + m = re.match(pattern_t, line) if m: last_tt = float(m.group(2)) - m=re.match(pattern_g, line) + m = re.match(pattern_g, line) if m and last_tt and last_tt > _min and last_tt < _max: list_tt.append([last_tt, float(m.group(2)), float(m.group(3)), float(m.group(4))]) return scipy.array(list_tt) diff --git a/sw/tools/dfu/stm32_mem.py b/sw/tools/dfu/stm32_mem.py index 9e23af6b02..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') @@ -108,6 +123,8 @@ if __name__ == "__main__": print('.', end="") stdout.flush() time.sleep(0.5) + else: + break print("") if not devs: print("No DFU devices found!") @@ -119,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 = [] @@ -150,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: @@ -191,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 8c5a0cd249..404a1fbe5f 100644 --- a/sw/tools/gen_flight_plan.ml +++ b/sw/tools/gen_flight_plan.ml @@ -808,6 +808,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 "[^A-Za-z0-9]") "_" 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 253182f8c3..1fdb99a7b4 100644 --- a/sw/tools/gen_periodic.ml +++ b/sw/tools/gen_periodic.ml @@ -142,6 +142,7 @@ let _ = fprintf out_h "#define _VAR_PERIODIC_H_\n\n"; fprintf out_h "#include \"std.h\"\n"; fprintf out_h "#include \"generated/airframe.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..2413805309 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 "[^A-Za-z0-9]") "_" v) !idx; incr idx) + settings; + lprintf "\n"; + (** Macro to call to set one variable *) lprintf "#define DlSetting(_idx, _value) { \\\n"; right (); diff --git a/sw/tools/process_exif/README b/sw/tools/process_exif/README new file mode 100644 index 0000000000..9e41eab5a6 --- /dev/null +++ b/sw/tools/process_exif/README @@ -0,0 +1,67 @@ + +This is a script which loads GPS info into photo files (EXIF) based on the DC_SHOT messages +in the telemetry data. The .data file contains DC_SHOT messages (possibly not all emitted by telemetry, +but I'll get back to that later). This file together with the files in jpeg format are +copied to a processing directory, where this script extracts the GPS position from each DC_SHOT message +and edits the exif photo data in place, so effectively loads the GPS position into the EXIF data for +each photo. + +This script allows for gaps in DC_SHOT photo numbers and has some rudimentary error checking. + + +Instructions for use (on Ubuntu): + +1. Make sure python is installed. +2. Install the necessary python packages: + +# sudo apt-get install python-gdal +# sudo apt-get install gir1.2-gexiv2-0.4 + +3. Create a directory for processing. The photo's EXIF data will be edited in place. +4. Copy the .data file from the /var/log directory into this processing directory. +5. Copy all photos taken during the session to the directory. + +Verification: +- Sort photos by name. Since most cameras output the photo name with a number at the end, this should + show a list of photo names with consecutive numbers. +- Verify that the first photo corresponds to the first DC_SHOT message (photonr == 1). +- Add some dummy photos at the start to make up for any missing photos that may exist (such that they get + sorted before the real actual one). + +6. Run the script: + +# python sw/tools/process_exif/process_exif.py / + +Verification of processing: +- Look at the logs! These include the GPS positions calculated from UTM paparazzi positions. + It has been tested on the southern+western hemispheres, but not yet on eastern and northern hemispheres. +- Verify the tags in the photo: + exiftool -v2 /.jpg + +---- + +Considerations: + +The Ivy telemetry bus can get a little clogged up and this will result in discarded messages. This means that +not all DC_SHOT messages actually emitted by telemetry need to be there on the ground station, so some photos +won't have GPS coordinates loaded. Not all processing software for orthomosaics however require GPS positions +for all photos, for example if they rely on recognizing corresponding features in overlapping photos. + +Having more GPS coordinates in photos helps to improve accuracy, as the error in measurements approximates zero +over time if the error follows a normal distribution. + +Having many GPS coordinates is not enough however to achieve correct precision down to centimeters. Atmospheric conditions +need to be eliminated by applying 3-5 ground control points in strategic locations. These conditions can easily +cause the entire orthomosaic to be shifted over a meter. + + +If your processing software does require the GPS coordinates per photo, you need to look into the use of a +telemetry logger onboard. This logger can be hooked up separately on the tx+gnd lines of telemetry and "listen in" on +the connection. + + +The timing between actually taking a picture and the pulse event being sent is also an important consideration. +Obviously the GPS frequency is also an issue. If this is used on a fixedwing, obviously if the plane is underway +at 12m/s, 250ms will introduce a 3m bias on the position. Another reason to rely on GPS positions only as hints +and apply ground control points to "set" the orthomosaic to the right location. + diff --git a/sw/tools/process_exif/process_exif.py b/sw/tools/process_exif/process_exif.py new file mode 100644 index 0000000000..9394245504 --- /dev/null +++ b/sw/tools/process_exif/process_exif.py @@ -0,0 +1,166 @@ +#!/usr/bin/python +# +# +# +# +# +# +# +# +# +# +# +# +# +# + +# sudo apt-get install python-gdal +# sudo apt-get install gir1.2-gexiv2-0.4 + +from gi.repository import GExiv2 +import glob +import os +import re +import fnmatch +import sys +import math + +M_PI=3.14159265358979323846 +M_PI_2=(M_PI/2) +M_PI_4=(M_PI/4) + +def RadOfDeg( deg ): + return (deg / M_PI) * 180. + +# converts UTM coords to lat/long. Equations from USGS Bulletin 1532 +# East Longitudes are positive, West longitudes are negative. +# North latitudes are positive, South latitudes are negative +# Lat and Long are in decimal degrees. +# Written by Chuck Gantz- chuck.gantz@globalstar.com + +# ( I had some code here to use GDAL and which looked much simpler, but couldn't get that to work ) + +def UTMtoLL( northing, easting, utm_zone ): + + k0 = 0.9996; + a = 6378137; # WGS-84 + eccSquared = 0.00669438; # WGS-84 + e1 = (1-math.sqrt(1-eccSquared))/(1+math.sqrt(1-eccSquared)); + + x = easting - 500000.0; # remove 500,000 meter offset for longitude + y = northing; + + is_northern = northing < 0 + if ( not is_northern ): + y -= 10000000.0 # remove 10,000,000 meter offset used for southern hemisphere + + LongOrigin = (utm_zone - 1)*6 - 180 + 3; # +3 puts origin in middle of zone + + eccPrimeSquared = (eccSquared)/(1-eccSquared); + + M = y / k0; + mu = M/(a*(1-eccSquared/4-3*eccSquared*eccSquared/64-5*eccSquared*eccSquared*eccSquared/256)); + + phi1Rad = mu + (3*e1/2-27*e1*e1*e1/32)*math.sin(2*mu) + (21*e1*e1/16-55*e1*e1*e1*e1/32)*math.sin(4*mu) +(151*e1*e1*e1/96)*math.sin(6*mu); + phi1 = RadOfDeg(phi1Rad); + + N1 = a/math.sqrt(1-eccSquared*math.sin(phi1Rad)*math.sin(phi1Rad)); + T1 = math.tan(phi1Rad)*math.tan(phi1Rad); + C1 = eccPrimeSquared*math.cos(phi1Rad)*math.cos(phi1Rad); + R1 = a*(1-eccSquared)/math.pow(1-eccSquared*math.sin(phi1Rad)*math.sin(phi1Rad), 1.5); + D = x/(N1*k0); + + Lat = phi1Rad - (N1*math.tan(phi1Rad)/R1)*(D*D/2-(5+3*T1+10*C1-4*C1*C1-9*eccPrimeSquared)*D*D*D*D/24+(61+90*T1+298*C1+45*T1*T1-252*eccPrimeSquared-3*C1*C1)*D*D*D*D*D*D/720); + Lat = RadOfDeg(Lat) + + Long = (D-(1+2*T1+C1)*D*D*D/6+(5-2*C1+28*T1-3*C1*C1+8*eccPrimeSquared+24*T1*T1)*D*D*D*D*D/120)/math.cos(phi1Rad) + Long = LongOrigin + RadOfDeg(Long) + + return Lat, Long + + +# At least the directory must be given +if len(sys.argv) < 2: + print "This script requires one argument: A directory containing photos and the paparazzi .data file" + sys.exit() + +path = str(sys.argv[ 1] ) + +if os.path.isdir(path) == False: + print "The indicated path '%s' is not a directory"%(path) + sys.exit() + +# Searching for all files with .data extension in indicated directory. +# It should only have one. +list_path = [i for i in os.listdir(path) if os.path.isfile(os.path.join(path, i))] +files = [os.path.join(path, j) for j in list_path if re.match(fnmatch.translate('*.data'), j, re.IGNORECASE)] + +if len(files) > 1: + print "Too many data files found. Only one is allowed." + sys.exit() + +if len(files) == 0: + print "No data files in 'data'. Copy data file there." + sys.exit() + +# Now searching for all photos (extension .jpg) in directory +list_path = [i for i in os.listdir(path) if os.path.isfile(os.path.join(path, i))] +photos = [os.path.join(path, j) for j in list_path if re.match(fnmatch.translate('*.jpg'), j, re.IGNORECASE)] + +# Photos must be sorted by number +photos.sort() + +# Opening the data file, iterating all lines and searching for DC_SHOT messages +f = open( files[0], 'r' ) +for line in f: + line = line.rstrip() + line = re.sub(' +',' ',line) + if 'DC_SHOT' in line: + # 618.710 1 DC_SHOT 212 29133350 -89510400 8.5 25 -9 29 0 0 385051650 + splitted = line.split( ' ' ) + + if len(splitted) < 12: + continue + try: + photonr = int(splitted[ 3 ]) + utm_east = ( float(int(splitted[ 4 ])) / 100. ) + utm_north = ( float(int(splitted[ 5 ])) / 100. ) + alt = float(splitted[ 6 ]) + utm_zone = int(splitted[ 7 ]) + phi = int(splitted[ 8 ]) + theta = int(splitted[ 9 ]) + course = int(splitted[ 10 ]) + speed = int(splitted[ 11 ]) + + lon, lat = UTMtoLL( utm_north, utm_east, utm_zone ) + + # Check that there as many photos and pick the indicated one. + # (this assumes the photos were taken correctly without a hiccup) + # It would never be able to check this anyway, since the camera could stall or + # not interpret the pulse? Leading to an incorrect GPS coordinate. + if len( photos ) < photonr: + print "Photo data %d found, but ran out of photos in directory"%(photonr) + continue + + # I've seen log files with -1 as DC_SHOT number due to an int8 I think. This should be + # fixed now, but just in case someone runs this on old data. + if (photonr < 0): + print "Negative photonr found." + continue + + # Pick out photo, open it through exiv2, + photoname = photos[ photonr - 1 ] + photo = GExiv2.Metadata( photoname ) + + photo.set_gps_info(lat, lon, alt) + photo.save_file() + + print "Photo %s and photonr %d merged. Lat/Lon/Alt: %f, %f, %f"%(photoname, photonr, lat, lon, alt) + + except ValueError as e: + print "Cannot read line: %s"%(line) + print "Value error(%s)"%(e) + continue + +print "Finished! exiting." + 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;